From c4ed97f3c16311601b4983360d51f662729e81d4 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Thu, 5 Jun 2014 07:06:48 -0400
Subject: [PATCH 001/416] Added uORB tiny pub/sub for usage without struct on
 stack.

---
 src/drivers/roboclaw/RoboClaw.cpp             |   2 +-
 src/modules/controllib/block/Block.cpp        |   4 +-
 src/modules/controllib/block/Block.hpp        |  12 +-
 src/modules/controllib/uorb/blocks.cpp        |  18 +-
 .../mc_att_control/mc_att_control_main.cpp    | 207 ++++--------------
 src/modules/uORB/Publication.cpp              |   6 +-
 src/modules/uORB/Publication.hpp              |  96 ++++++--
 src/modules/uORB/Subscription.cpp             |  16 +-
 src/modules/uORB/Subscription.hpp             | 118 +++++++---
 9 files changed, 236 insertions(+), 243 deletions(-)

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index dd5e4d3e0c..280f6c831d 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -65,7 +65,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
 	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
 	_controlPoll(),
-	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_actuators(ORB_ID(actuator_controls_0), 20),
 	_motor1Position(0),
 	_motor1Speed(0),
 	_motor1Overflow(0),
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 6768bfa7ed..f3cd877287 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -101,7 +101,7 @@ void Block::updateParams()
 
 void Block::updateSubscriptions()
 {
-	uORB::SubscriptionBase *sub = getSubscriptions().getHead();
+	uORB::SubscriptionNode *sub = getSubscriptions().getHead();
 	int count = 0;
 
 	while (sub != NULL) {
@@ -119,7 +119,7 @@ void Block::updateSubscriptions()
 
 void Block::updatePublications()
 {
-	uORB::PublicationBase *pub = getPublications().getHead();
+	uORB::PublicationNode *pub = getPublications().getHead();
 	int count = 0;
 
 	while (pub != NULL) {
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 736698e213..42ca3a742e 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -46,8 +46,8 @@
 
 // forward declaration
 namespace uORB {
-	class SubscriptionBase;
-	class PublicationBase;
+	class SubscriptionNode;
+	class PublicationNode;
 }
 
 namespace control
@@ -83,15 +83,15 @@ public:
 protected:
 // accessors
 	SuperBlock *getParent() { return _parent; }
-	List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
-	List<uORB::PublicationBase *> & getPublications() { return _publications; }
+	List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
+	List<uORB::PublicationNode *> & getPublications() { return _publications; }
 	List<BlockParamBase *> & getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt;
-	List<uORB::SubscriptionBase *> _subscriptions;
-	List<uORB::PublicationBase *> _publications;
+	List<uORB::SubscriptionNode *> _subscriptions;
+	List<uORB::PublicationNode *> _publications;
 	List<BlockParamBase *> _params;
 };
 
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index e8fecef0d9..454d0db19c 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
 BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	// subscriptions
-	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
-	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
-	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
-	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
-	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
-	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
+	_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
+	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
+	_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
+	_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
+	_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
+	_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
+	_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
 	// publications
-	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+	_actuators(ORB_ID(actuator_controls_0), &getPublications())
 {
 }
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 20e016da34..b024589a6f 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -60,6 +60,8 @@
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -111,17 +113,17 @@ private:
 	bool	_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
-	int		_v_att_sub;				/**< vehicle attitude subscription */
-	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
-	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
-	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
-	int		_params_sub;			/**< parameter updates subscription */
-	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
-	int		_armed_sub;				/**< arming status subscription */
+	uORB::SubscriptionTiny		_v_att_sub;				/**< vehicle attitude subscription */
+	uORB::SubscriptionTiny		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
+	uORB::SubscriptionTiny		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
+	uORB::SubscriptionTiny		_v_control_mode_sub;	/**< vehicle control mode subscription */
+	uORB::SubscriptionTiny		_params_sub;			/**< parameter updates subscription */
+	uORB::SubscriptionTiny		_manual_control_sp_sub;	/**< manual control setpoint subscription */
+	uORB::SubscriptionTiny		_armed_sub;				/**< arming status subscription */
 
-	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
-	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
-	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+	uORB::PublicationTiny	_att_sp_pub;			/**< attitude setpoint publication */
+	uORB::PublicationTiny	_v_rates_sp_pub;		/**< rate setpoint publication */
+	uORB::PublicationTiny	_actuators_0_pub;		/**< attitude actuator controls publication */
 
 	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
@@ -186,36 +188,6 @@ private:
 	 */
 	int			parameters_update();
 
-	/**
-	 * Check for parameter update and handle it.
-	 */
-	void		parameter_update_poll();
-
-	/**
-	 * Check for changes in vehicle control mode.
-	 */
-	void		vehicle_control_mode_poll();
-
-	/**
-	 * Check for changes in manual inputs.
-	 */
-	void		vehicle_manual_poll();
-
-	/**
-	 * Check for attitude setpoint updates.
-	 */
-	void		vehicle_attitude_setpoint_poll();
-
-	/**
-	 * Check for rates setpoint updates.
-	 */
-	void		vehicle_rates_setpoint_poll();
-
-	/**
-	 * Check for arming status updates.
-	 */
-	void		arming_status_poll();
-
 	/**
 	 * Attitude controller.
 	 */
@@ -255,17 +227,18 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_control_task(-1),
 
 /* subscriptions */
-	_v_att_sub(-1),
-	_v_att_sp_sub(-1),
-	_v_control_mode_sub(-1),
-	_params_sub(-1),
-	_manual_control_sp_sub(-1),
-	_armed_sub(-1),
+	_v_att_sub(ORB_ID(vehicle_attitude)),
+	_v_att_sp_sub(ORB_ID(vehicle_attitude_setpoint)),
+	_v_rates_sp_sub(ORB_ID(vehicle_rates_setpoint)),
+	_v_control_mode_sub(ORB_ID(vehicle_control_mode)),
+	_params_sub(ORB_ID(parameter_update)),
+	_manual_control_sp_sub(ORB_ID(manual_control_setpoint)),
+	_armed_sub(ORB_ID(actuator_armed)),
 
 /* publications */
-	_att_sp_pub(-1),
-	_v_rates_sp_pub(-1),
-	_actuators_0_pub(-1),
+	_att_sp_pub(ORB_ID(vehicle_attitude_setpoint)),
+	_v_rates_sp_pub(ORB_ID(vehicle_rates_setpoint)),
+	_actuators_0_pub(ORB_ID_VEHICLE_ATTITUDE_CONTROLS),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -405,83 +378,6 @@ MulticopterAttitudeControl::parameters_update()
 	return OK;
 }
 
-void
-MulticopterAttitudeControl::parameter_update_poll()
-{
-	bool updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_params_sub, &updated);
-
-	if (updated) {
-		struct parameter_update_s param_update;
-		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
-		parameters_update();
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_control_mode_poll()
-{
-	bool updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_v_control_mode_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_manual_poll()
-{
-	bool updated;
-
-	/* get pilots inputs */
-	orb_check(_manual_control_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_att_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_rates_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::arming_status_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_armed_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-	}
-}
-
 /*
  * Attitude controller.
  * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -498,7 +394,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			vehicle_attitude_setpoint_poll();
+			(void) _v_att_sp_sub.update(&_v_att_sp);
 		}
 
 		if (!_v_control_mode.flag_control_climb_rate_enabled) {
@@ -553,7 +449,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		vehicle_attitude_setpoint_poll();
+		(void) _v_att_sp_sub.update(&_v_att_sp);
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
@@ -581,12 +477,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	if (publish_att_sp) {
 		_v_att_sp.timestamp = hrt_absolute_time();
 
-		if (_att_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
-
-		} else {
-			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
-		}
+		_att_sp_pub.update(&_v_att_sp);
 	}
 
 	/* rotation matrix for current state */
@@ -717,24 +608,13 @@ MulticopterAttitudeControl::task_main()
 	warnx("started");
 	fflush(stdout);
 
-	/*
-	 * do subscriptions
-	 */
-	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-
 	/* initialize parameters cache */
 	parameters_update();
 
 	/* wakeup source: vehicle attitude */
 	struct pollfd fds[1];
 
-	fds[0].fd = _v_att_sub;
+	fds[0].fd = _v_att_sub.getHandle();
 	fds[0].events = POLLIN;
 
 	while (!_task_should_exit) {
@@ -771,13 +651,15 @@ MulticopterAttitudeControl::task_main()
 			}
 
 			/* copy attitude topic */
-			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+			_v_att_sub.update(&_v_att);
 
 			/* check for updates in other topics */
-			parameter_update_poll();
-			vehicle_control_mode_poll();
-			arming_status_poll();
-			vehicle_manual_poll();
+			if (_params_sub.updated()) {
+				parameters_update();
+			}
+			(void) _v_control_mode_sub.update(&_v_control_mode);
+			(void) _armed_sub.update(&_armed);
+			(void) _manual_control_sp_sub.update(&_manual_control_sp);
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -789,12 +671,7 @@ MulticopterAttitudeControl::task_main()
 				_v_rates_sp.thrust = _thrust_sp;
 				_v_rates_sp.timestamp = hrt_absolute_time();
 
-				if (_v_rates_sp_pub > 0) {
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-				} else {
-					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-				}
+				_v_rates_sp_pub.update(&_v_rates_sp);
 
 			} else {
 				/* attitude controller disabled, poll rates setpoint topic */
@@ -813,16 +690,11 @@ MulticopterAttitudeControl::task_main()
 					_v_rates_sp.thrust = _thrust_sp;
 					_v_rates_sp.timestamp = hrt_absolute_time();
 
-					if (_v_rates_sp_pub > 0) {
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-					} else {
-						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-					}
+					_v_rates_sp_pub.update(&_v_rates_sp);
 
 				} else {
 					/* attitude controller disabled, poll rates setpoint topic */
-					vehicle_rates_setpoint_poll();
+					_v_rates_sp_sub.update(&_v_rates_sp);
 					_rates_sp(0) = _v_rates_sp.roll;
 					_rates_sp(1) = _v_rates_sp.pitch;
 					_rates_sp(2) = _v_rates_sp.yaw;
@@ -840,12 +712,7 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
 
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
-
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
-				}
+				_actuators_0_pub.update(&_actuators);
 			}
 		}
 
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index 5a59816172..e9398a6253 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -52,10 +52,10 @@ namespace uORB {
 
 template<class T>
 Publication<T>::Publication(
-	List<PublicationBase *> * list,
-	const struct orb_metadata *meta) :
+	const struct orb_metadata *meta,
+	List<PublicationNode *> * list) :
 	T(), // initialize data structure to zero
-	PublicationBase(list, meta) {
+	PublicationNode(meta, list) {
 }
 
 template<class T>
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index 8650b3df89..1c48929e41 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -38,6 +38,8 @@
 
 #pragma once
 
+#include <assert.h>
+
 #include <uORB/uORB.h>
 #include <containers/List.hpp>
 
@@ -49,55 +51,112 @@ namespace uORB
  * Base publication warapper class, used in list traversal
  * of various publications.
  */
-class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
+class __EXPORT PublicationBase
 {
 public:
 
-	PublicationBase(
-		List<PublicationBase *> * list,
-		const struct orb_metadata *meta) :
+	/**
+	 * Constructor
+	 *
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * 	macro) for the topic.
+	 */
+	PublicationBase(const struct orb_metadata *meta) :
 		_meta(meta),
 		_handle(-1) {
-		if (list != NULL) list->add(this);
 	}
-	void update() {
+
+	/**
+	 * Update the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	void update(void * data) {
 		if (_handle > 0) {
-			orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+			orb_publish(getMeta(), getHandle(), data);
 		} else {
-			setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+			setHandle(orb_advertise(getMeta(), data));
 		}
 	}
-	virtual void *getDataVoidPtr() = 0;
+
+	/**
+	 * Deconstructor
+	 */
 	virtual ~PublicationBase() {
 		orb_unsubscribe(getHandle());
 	}
+// accessors
 	const struct orb_metadata *getMeta() { return _meta; }
 	int getHandle() { return _handle; }
 protected:
+// accessors
 	void setHandle(orb_advert_t handle) { _handle = handle; }
+// attributes
 	const struct orb_metadata *_meta;
 	orb_advert_t _handle;
 };
 
+/** 
+ * alias class name so it is clear that the base class
+ * can be used by itself if desired
+ */
+typedef PublicationBase PublicationTiny;
+
+/**
+ * The publication base class as a list node.
+ */
+class __EXPORT PublicationNode :
+	public PublicationBase,
+	public ListNode<PublicationNode *>
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * 	macro) for the topic.
+	 * @param list 	A pointer to a list of subscriptions 
+	 * 	that this should be appended to.
+	 */
+	PublicationNode(const struct orb_metadata *meta,
+		List<PublicationNode *> * list=nullptr) :
+		PublicationBase(meta) {
+		if (list != nullptr) list->add(this);
+	}
+
+	/**
+	 * This function is the callback for list traversal
+	 * updates, a child class must implement it.
+	 */
+	virtual void update() = 0;
+};
+
 /**
  * Publication wrapper class
  */
 template<class T>
 class Publication :
 	public T, // this must be first!
-	public PublicationBase
+	public PublicationNode
 {
 public:
 	/**
 	 * Constructor
 	 *
-	 * @param list      A list interface for adding to list during construction
-	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
-	 *			for the topic.
+	 * @param meta The uORB metadata (usually from 
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param list A list interface for adding to
+	 * 	list during construction
 	 */
-	Publication(List<PublicationBase *> * list,
-		const struct orb_metadata *meta);
+	Publication(const struct orb_metadata *meta,
+		List<PublicationNode *> * list=nullptr);
+
+	/**
+	 * Deconstructor
+	 **/
 	virtual ~Publication();
+
 	/*
 	 * XXX
 	 * This function gets the T struct, assuming
@@ -106,6 +165,13 @@ public:
 	 * seem to be available
 	 */
 	void *getDataVoidPtr();
+
+	/**
+	 * Create an update function that uses the embedded struct.
+	 */
+	void update() {
+		PublicationBase::update(getDataVoidPtr());
+	}
 };
 
 } // namespace uORB
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index c1d1a938f1..1a4867c853 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -55,21 +55,13 @@
 namespace uORB
 {
 
-bool __EXPORT SubscriptionBase::updated()
-{
-	bool isUpdated = false;
-	orb_check(_handle, &isUpdated);
-	return isUpdated;
-}
-
 template<class T>
 Subscription<T>::Subscription(
-	List<SubscriptionBase *> * list,
-	const struct orb_metadata *meta, unsigned interval) :
+	const struct orb_metadata *meta,
+	unsigned interval,
+	List<SubscriptionNode *> * list) :
 	T(), // initialize data structure to zero
-	SubscriptionBase(list, meta) {
-	setHandle(orb_subscribe(getMeta()));
-	orb_set_interval(getHandle(), interval);
+	SubscriptionNode(meta, interval, list) {
 }
 
 template<class T>
diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index 34e9a83e03..bab1ef2ea5 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -38,10 +38,11 @@
 
 #pragma once
 
+#include <assert.h>
+
 #include <uORB/uORB.h>
 #include <containers/List.hpp>
 
-
 namespace uORB
 {
 
@@ -49,8 +50,7 @@ namespace uORB
  * Base subscription warapper class, used in list traversal
  * of various subscriptions.
  */
-class __EXPORT SubscriptionBase :
-	public ListNode<SubscriptionBase *>
+class __EXPORT SubscriptionBase
 {
 public:
 // methods
@@ -58,23 +58,42 @@ public:
 	/**
 	 * Constructor
 	 *
-	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
-	 *			for the topic.
+	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * 	macro) for the topic.
+	 *
+	 * @param interval  The minimum interval in milliseconds 
+	 * 	between updates
 	 */
-	SubscriptionBase(
-		List<SubscriptionBase *> * list,
-		const struct orb_metadata *meta) :
+	SubscriptionBase(const struct orb_metadata *meta,
+		unsigned interval=0) :
 		_meta(meta),
 		_handle() {
-		if (list != NULL) list->add(this);
+		setHandle(orb_subscribe(getMeta()));
+		orb_set_interval(getHandle(), interval);
 	}
-	bool updated();
-	void update() {
+
+	/**
+	 * Check if there is a new update.
+	 * */
+	bool updated() {
+		bool isUpdated = false;
+		orb_check(_handle, &isUpdated);
+		return isUpdated;
+	}
+
+	/**
+	 * Update the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	void update(void * data) {
 		if (updated()) {
-			orb_copy(_meta, _handle, getDataVoidPtr());
+			orb_copy(_meta, _handle, data);
 		}
 	}
-	virtual void *getDataVoidPtr() = 0;
+
+	/**
+	 * Deconstructor
+	 */
 	virtual ~SubscriptionBase() {
 		orb_unsubscribe(_handle);
 	}
@@ -90,30 +109,79 @@ protected:
 };
 
 /**
- * Subscription wrapper class
+ * alias class name so it is clear that the base class
  */
-template<class T>
-class __EXPORT Subscription :
-	public T, // this must be first!
-	public SubscriptionBase
+typedef SubscriptionBase SubscriptionTiny;
+
+/**
+ * The publication base class as a list node.
+ */
+class __EXPORT SubscriptionNode :
+
+	public SubscriptionBase,
+	public ListNode<SubscriptionNode *>
 {
 public:
 	/**
 	 * Constructor
 	 *
-	 * @param list      A list interface for adding to list during construction
-	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
-	 *			for the topic.
-	 * @param interval  The minimum interval in milliseconds between updates
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * 	macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds 
+	 * 	between updates
+	 * @param list 	A pointer to a list of subscriptions 
+	 * 	that this should be appended to.
 	 */
-	Subscription(
-		List<SubscriptionBase *> * list,
-		const struct orb_metadata *meta, unsigned interval);
+	SubscriptionNode(const struct orb_metadata *meta,
+		unsigned interval=0,
+		List<SubscriptionNode *> * list=nullptr) :
+		SubscriptionBase(meta, interval) {
+		if (list != nullptr) list->add(this);
+	}
+
+	/**
+	 * This function is the callback for list traversal
+	 * updates, a child class must implement it.
+	 */
+	virtual void update() = 0;
+};
+
+/**
+ * Subscription wrapper class
+ */
+template<class T>
+class __EXPORT Subscription :
+	public T, // this must be first!
+	public SubscriptionNode
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from 
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds 
+	 * 	between updates
+	 * @param list A list interface for adding to 
+	 * 	list during construction
+	 */
+	Subscription(const struct orb_metadata *meta, 
+		unsigned interval=0,
+		List<SubscriptionNode *> * list=nullptr);
 	/**
 	 * Deconstructor
 	 */
 	virtual ~Subscription();
 
+
+	/**
+	 * Create an update function that uses the embedded struct.
+	 */
+	void update() {
+		SubscriptionBase::update(getDataVoidPtr());
+	}
+
 	/*
 	 * XXX
 	 * This function gets the T struct, assuming

From cedfdfca6018c7c35a56e2370b84f0d4f589cf68 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Fri, 22 Aug 2014 16:00:34 +0200
Subject: [PATCH 002/416] some hacks for the quadshot

---
 ROMFS/px4fmu_common/init.d/13000_quadshot          | 14 ++++++++++++++
 .../px4fmu_common/init.d/Roman_mavlink_stream_conf | 12 ++++++++++++
 ROMFS/px4fmu_common/init.d/rc.autostart            |  9 +++++++++
 ROMFS/px4fmu_common/init.d/rcS                     |  1 +
 4 files changed, 36 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot
 create mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf

diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot
new file mode 100644
index 0000000000..8ee306a387
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/13000_quadshot
@@ -0,0 +1,14 @@
+#!nsh
+#
+# Generic quadshot configuration file
+#
+# Roman Bapst <romanbapst@yahoo.de>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER FMU_quadshot
+
+set PWM_OUTPUTS 1234
+set PWM_MIN 1070
+set PWM_MAX 2000
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf
new file mode 100644
index 0000000000..d26e4a3722
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf
@@ -0,0 +1,12 @@
+#!nsh
+# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard
+#
+#usleep 100000
+#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50
+#usleep 100000
+#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50
+#usleep 100000
+#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50
+usleep 100000
+mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50
+echo "Added additional streams on TELEM2"
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 9de7d9ecdb..4f0ec02787 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -16,6 +16,7 @@
 # 10000	..	10999		Wide arm / H frame
 # 11000	..	11999		Hexa Cox
 # 12000	..	12999		Octo Cox
+# 13000 ..  13999		Vtol
 
 #
 # Simulation setups
@@ -232,3 +233,11 @@ if param compare SYS_AUTOSTART 12001
 then
 	sh /etc/init.d/12001_octo_cox
 fi
+
+#
+# Quadshot
+#
+ if param compare SYS_AUTOSTART 13000
+ then
+ 	 sh /etc/init.d/13000_quadshot
+ fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c9e6a27cac..cd1aa14b0c 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -586,6 +586,7 @@ then
 	then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
+		sh /etc/init.d/Roman_mavlink_stream_conf
 	else
 		echo "[init] No addons script: $EXTRAS_FILE"
 	fi

From cce45865d53eab4d298e8a22671f6eb6fb32801c Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 28 Aug 2014 10:57:59 +0200
Subject: [PATCH 003/416] NuttX update

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index 41fffa0df1..088146b90e 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c
+Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172

From 87259cf89de207b3cb21ab9f44578fd2c21ce119 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 28 Aug 2014 10:58:46 +0200
Subject: [PATCH 004/416] added updates

---
 .catkin_workspace                             |   1 +
 build/CATKIN_IGNORE                           |   0
 build/CMakeCache.txt                          | 450 ++++++++++++++++++
 .../CMakeFiles/2.8.12.2/CMakeCCompiler.cmake  |  56 +++
 .../2.8.12.2/CMakeCXXCompiler.cmake           |  57 +++
 .../2.8.12.2/CMakeDetermineCompilerABI_C.bin  | Bin 0 -> 8547 bytes
 .../CMakeDetermineCompilerABI_CXX.bin         | Bin 0 -> 8560 bytes
 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake   |  15 +
 .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c   | 389 +++++++++++++++
 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out   | Bin 0 -> 8643 bytes
 .../CompilerIdCXX/CMakeCXXCompilerId.cpp      | 377 +++++++++++++++
 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin 0 -> 8652 bytes
 .../CMakeDirectoryInformation.cmake           |  16 +
 build/CMakeFiles/CMakeError.log               |  53 +++
 build/CMakeFiles/CMakeOutput.log              | 293 ++++++++++++
 build/CMakeFiles/CMakeRuleHashes.txt          |   5 +
 build/CMakeFiles/Makefile.cmake               | 150 ++++++
 build/CMakeFiles/Makefile2                    | 266 +++++++++++
 build/CMakeFiles/TargetDirectories.txt        |   6 +
 .../clean_test_results.dir/DependInfo.cmake   |  20 +
 .../clean_test_results.dir/build.make         |  66 +++
 .../clean_test_results.dir/cmake_clean.cmake  |   8 +
 .../clean_test_results.dir/progress.make      |   1 +
 build/CMakeFiles/cmake.check_cache            |   1 +
 build/CMakeFiles/doxygen.dir/DependInfo.cmake |  20 +
 build/CMakeFiles/doxygen.dir/build.make       |  65 +++
 .../CMakeFiles/doxygen.dir/cmake_clean.cmake  |   8 +
 build/CMakeFiles/doxygen.dir/progress.make    |   1 +
 build/CMakeFiles/progress.marks               |   1 +
 .../CMakeFiles/run_tests.dir/DependInfo.cmake |  20 +
 build/CMakeFiles/run_tests.dir/build.make     |  65 +++
 .../run_tests.dir/cmake_clean.cmake           |   8 +
 build/CMakeFiles/run_tests.dir/progress.make  |   1 +
 build/CMakeFiles/tests.dir/DependInfo.cmake   |  20 +
 build/CMakeFiles/tests.dir/build.make         |  65 +++
 build/CMakeFiles/tests.dir/cmake_clean.cmake  |   8 +
 build/CMakeFiles/tests.dir/progress.make      |   1 +
 build/CTestTestfile.cmake                     |   7 +
 build/Makefile                                | 262 ++++++++++
 .../catkin_generated/version/package.cmake    |   9 +
 build/catkin_generated/env_cached.sh          |  16 +
 .../catkin_generated/generate_cached_setup.py |  29 ++
 .../catkin_generated/installspace/.rosinstall |   2 +
 .../installspace/_setup_util.py               | 287 +++++++++++
 build/catkin_generated/installspace/env.sh    |  16 +
 .../catkin_generated/installspace/setup.bash  |   8 +
 build/catkin_generated/installspace/setup.sh  |  87 ++++
 build/catkin_generated/installspace/setup.zsh |   8 +
 build/catkin_generated/order_packages.cmake   |  10 +
 build/catkin_generated/order_packages.py      |   5 +
 build/catkin_generated/setup_cached.sh        |  20 +
 .../Project/interrogate_setup_dot_py.py.stamp | 250 ++++++++++
 .../Project/order_packages.cmake.em.stamp     |  56 +++
 .../stamps/Project/package.xml.stamp          |  37 ++
 build/catkin_make.cache                       |   1 +
 build/cmake_install.cmake                     | 140 ++++++
 .../CMakeDirectoryInformation.cmake           |  16 +
 .../CMakeFiles/gtest.dir/DependInfo.cmake     |  27 ++
 build/gtest/CMakeFiles/gtest.dir/build.make   | 102 ++++
 .../CMakeFiles/gtest.dir/cmake_clean.cmake    |  10 +
 build/gtest/CMakeFiles/gtest.dir/depend.make  |   2 +
 build/gtest/CMakeFiles/gtest.dir/flags.make   |   8 +
 build/gtest/CMakeFiles/gtest.dir/link.txt     |   1 +
 .../gtest/CMakeFiles/gtest.dir/progress.make  |   2 +
 .../gtest_main.dir/DependInfo.cmake           |  28 ++
 .../CMakeFiles/gtest_main.dir/build.make      | 103 ++++
 .../gtest_main.dir/cmake_clean.cmake          |  10 +
 .../CMakeFiles/gtest_main.dir/depend.make     |   2 +
 .../CMakeFiles/gtest_main.dir/flags.make      |   8 +
 .../gtest/CMakeFiles/gtest_main.dir/link.txt  |   1 +
 .../CMakeFiles/gtest_main.dir/progress.make   |   2 +
 build/gtest/CMakeFiles/progress.marks         |   1 +
 build/gtest/CTestTestfile.cmake               |   6 +
 build/gtest/Makefile                          | 262 ++++++++++
 build/gtest/cmake_install.cmake               |  34 ++
 devel/.catkin                                 |   1 +
 devel/.rosinstall                             |   2 +
 devel/_setup_util.py                          | 287 +++++++++++
 devel/env.sh                                  |  16 +
 devel/setup.bash                              |   8 +
 devel/setup.sh                                |  87 ++++
 devel/setup.zsh                               |   8 +
 mavlink/include/mavlink/v1.0                  |   2 +-
 uavcan                                        |   2 +-
 84 files changed, 4799 insertions(+), 2 deletions(-)
 create mode 100644 .catkin_workspace
 create mode 100644 build/CATKIN_IGNORE
 create mode 100644 build/CMakeCache.txt
 create mode 100644 build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake
 create mode 100644 build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake
 create mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin
 create mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin
 create mode 100644 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake
 create mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c
 create mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out
 create mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
 create mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out
 create mode 100644 build/CMakeFiles/CMakeDirectoryInformation.cmake
 create mode 100644 build/CMakeFiles/CMakeError.log
 create mode 100644 build/CMakeFiles/CMakeOutput.log
 create mode 100644 build/CMakeFiles/CMakeRuleHashes.txt
 create mode 100644 build/CMakeFiles/Makefile.cmake
 create mode 100644 build/CMakeFiles/Makefile2
 create mode 100644 build/CMakeFiles/TargetDirectories.txt
 create mode 100644 build/CMakeFiles/clean_test_results.dir/DependInfo.cmake
 create mode 100644 build/CMakeFiles/clean_test_results.dir/build.make
 create mode 100644 build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake
 create mode 100644 build/CMakeFiles/clean_test_results.dir/progress.make
 create mode 100644 build/CMakeFiles/cmake.check_cache
 create mode 100644 build/CMakeFiles/doxygen.dir/DependInfo.cmake
 create mode 100644 build/CMakeFiles/doxygen.dir/build.make
 create mode 100644 build/CMakeFiles/doxygen.dir/cmake_clean.cmake
 create mode 100644 build/CMakeFiles/doxygen.dir/progress.make
 create mode 100644 build/CMakeFiles/progress.marks
 create mode 100644 build/CMakeFiles/run_tests.dir/DependInfo.cmake
 create mode 100644 build/CMakeFiles/run_tests.dir/build.make
 create mode 100644 build/CMakeFiles/run_tests.dir/cmake_clean.cmake
 create mode 100644 build/CMakeFiles/run_tests.dir/progress.make
 create mode 100644 build/CMakeFiles/tests.dir/DependInfo.cmake
 create mode 100644 build/CMakeFiles/tests.dir/build.make
 create mode 100644 build/CMakeFiles/tests.dir/cmake_clean.cmake
 create mode 100644 build/CMakeFiles/tests.dir/progress.make
 create mode 100644 build/CTestTestfile.cmake
 create mode 100644 build/Makefile
 create mode 100644 build/catkin/catkin_generated/version/package.cmake
 create mode 100755 build/catkin_generated/env_cached.sh
 create mode 100644 build/catkin_generated/generate_cached_setup.py
 create mode 100644 build/catkin_generated/installspace/.rosinstall
 create mode 100755 build/catkin_generated/installspace/_setup_util.py
 create mode 100755 build/catkin_generated/installspace/env.sh
 create mode 100644 build/catkin_generated/installspace/setup.bash
 create mode 100644 build/catkin_generated/installspace/setup.sh
 create mode 100644 build/catkin_generated/installspace/setup.zsh
 create mode 100644 build/catkin_generated/order_packages.cmake
 create mode 100644 build/catkin_generated/order_packages.py
 create mode 100755 build/catkin_generated/setup_cached.sh
 create mode 100644 build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp
 create mode 100644 build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp
 create mode 100644 build/catkin_generated/stamps/Project/package.xml.stamp
 create mode 100644 build/catkin_make.cache
 create mode 100644 build/cmake_install.cmake
 create mode 100644 build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/build.make
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/depend.make
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/flags.make
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/link.txt
 create mode 100644 build/gtest/CMakeFiles/gtest.dir/progress.make
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/build.make
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/depend.make
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/flags.make
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/link.txt
 create mode 100644 build/gtest/CMakeFiles/gtest_main.dir/progress.make
 create mode 100644 build/gtest/CMakeFiles/progress.marks
 create mode 100644 build/gtest/CTestTestfile.cmake
 create mode 100644 build/gtest/Makefile
 create mode 100644 build/gtest/cmake_install.cmake
 create mode 100644 devel/.catkin
 create mode 100644 devel/.rosinstall
 create mode 100755 devel/_setup_util.py
 create mode 100755 devel/env.sh
 create mode 100644 devel/setup.bash
 create mode 100644 devel/setup.sh
 create mode 100644 devel/setup.zsh

diff --git a/.catkin_workspace b/.catkin_workspace
new file mode 100644
index 0000000000..52fd97e7ea
--- /dev/null
+++ b/.catkin_workspace
@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration
diff --git a/build/CATKIN_IGNORE b/build/CATKIN_IGNORE
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt
new file mode 100644
index 0000000000..8fc5c4b4a8
--- /dev/null
+++ b/build/CMakeCache.txt
@@ -0,0 +1,450 @@
+# This is the CMakeCache file.
+# For build in directory: /home/roman/src/Firmware/build
+# It was generated by CMake: /usr/bin/cmake
+# You can edit this file to change values found and used by cmake.
+# If you do not want to change any of the values, simply exit the editor.
+# If you do want to change a value, simply edit, save, and exit the editor.
+# The syntax for the file is as follows:
+# KEY:TYPE=VALUE
+# KEY is the name of a variable in the cache.
+# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!.
+# VALUE is the current value for the KEY.
+
+########################
+# EXTERNAL cache entries
+########################
+
+//Build shared libraries (DLLs).
+BUILD_SHARED_LIBS:BOOL=ON
+
+//List of ';' separated packages to exclude
+CATKIN_BLACKLIST_PACKAGES:STRING=
+
+//catkin devel space
+CATKIN_DEVEL_PREFIX:PATH=/home/roman/src/Firmware/devel
+
+//Catkin enable testing
+CATKIN_ENABLE_TESTING:BOOL=ON
+
+//Catkin skip testing
+CATKIN_SKIP_TESTING:BOOL=OFF
+
+//List of ';' separated packages to build
+CATKIN_WHITELIST_PACKAGES:STRING=
+
+//Path to a program.
+CMAKE_AR:FILEPATH=/usr/bin/ar
+
+//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or
+// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel.
+CMAKE_BUILD_TYPE:STRING=
+
+//Enable/Disable color output during build.
+CMAKE_COLOR_MAKEFILE:BOOL=ON
+
+//CXX compiler.
+CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++
+
+//Flags used by the compiler during all build types.
+CMAKE_CXX_FLAGS:STRING=
+
+//Flags used by the compiler during debug builds.
+CMAKE_CXX_FLAGS_DEBUG:STRING=-g
+
+//Flags used by the compiler during release minsize builds.
+CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
+
+//Flags used by the compiler during release builds (/MD /Ob1 /Oi
+// /Ot /Oy /Gs will produce slightly less optimized but smaller
+// files).
+CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
+
+//Flags used by the compiler during Release with Debug Info builds.
+CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
+
+//C compiler.
+CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc
+
+//Flags used by the compiler during all build types.
+CMAKE_C_FLAGS:STRING=
+
+//Flags used by the compiler during debug builds.
+CMAKE_C_FLAGS_DEBUG:STRING=-g
+
+//Flags used by the compiler during release minsize builds.
+CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
+
+//Flags used by the compiler during release builds (/MD /Ob1 /Oi
+// /Ot /Oy /Gs will produce slightly less optimized but smaller
+// files).
+CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
+
+//Flags used by the compiler during Release with Debug Info builds.
+CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
+
+//Flags used by the linker.
+CMAKE_EXE_LINKER_FLAGS:STRING=' '
+
+//Flags used by the linker during debug builds.
+CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING=
+
+//Flags used by the linker during release minsize builds.
+CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING=
+
+//Flags used by the linker during release builds.
+CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING=
+
+//Flags used by the linker during Release with Debug Info builds.
+CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
+
+//Enable/Disable output of compile commands during generation.
+CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF
+
+//Install path prefix, prepended onto install directories.
+CMAKE_INSTALL_PREFIX:PATH=/home/roman/src/Firmware/install
+
+//Path to a program.
+CMAKE_LINKER:FILEPATH=/usr/bin/ld
+
+//Path to a program.
+CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make
+
+//Flags used by the linker during the creation of modules.
+CMAKE_MODULE_LINKER_FLAGS:STRING=' '
+
+//Flags used by the linker during debug builds.
+CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING=
+
+//Flags used by the linker during release minsize builds.
+CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING=
+
+//Flags used by the linker during release builds.
+CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING=
+
+//Flags used by the linker during Release with Debug Info builds.
+CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
+
+//Path to a program.
+CMAKE_NM:FILEPATH=/usr/bin/nm
+
+//Path to a program.
+CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy
+
+//Path to a program.
+CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump
+
+//Value Computed by CMake
+CMAKE_PROJECT_NAME:STATIC=Project
+
+//Path to a program.
+CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib
+
+//Flags used by the linker during the creation of dll's.
+CMAKE_SHARED_LINKER_FLAGS:STRING=' '
+
+//Flags used by the linker during debug builds.
+CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING=
+
+//Flags used by the linker during release minsize builds.
+CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING=
+
+//Flags used by the linker during release builds.
+CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING=
+
+//Flags used by the linker during Release with Debug Info builds.
+CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING=
+
+//If set, runtime paths are not added when installing shared libraries,
+// but are added when building.
+CMAKE_SKIP_INSTALL_RPATH:BOOL=NO
+
+//If set, runtime paths are not added when using shared libraries.
+CMAKE_SKIP_RPATH:BOOL=NO
+
+//Flags used by the linker during the creation of static libraries.
+CMAKE_STATIC_LINKER_FLAGS:STRING=
+
+//Flags used by the linker during debug builds.
+CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING=
+
+//Flags used by the linker during release minsize builds.
+CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING=
+
+//Flags used by the linker during release builds.
+CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING=
+
+//Flags used by the linker during Release with Debug Info builds.
+CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING=
+
+//Path to a program.
+CMAKE_STRIP:FILEPATH=/usr/bin/strip
+
+//If true, cmake will use relative paths in makefiles and projects.
+CMAKE_USE_RELATIVE_PATHS:BOOL=OFF
+
+//If this value is on, makefiles will be generated without the
+// .SILENT directive, and all commands will be echoed to the console
+// during the make.  This is useful for debugging only. With Visual
+// Studio IDE projects all commands are done without /nologo.
+CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE
+
+//Path to a program.
+DOXYGEN_EXECUTABLE:FILEPATH=DOXYGEN_EXECUTABLE-NOTFOUND
+
+//Path to a program.
+EMPY_EXECUTABLE:FILEPATH=/usr/bin/empy
+
+//Empy script
+EMPY_SCRIPT:STRING=/usr/bin/empy
+
+//Path to a file.
+GTEST_INCLUDE_DIR:PATH=/usr/include
+
+//Path to a library.
+GTEST_LIBRARY:FILEPATH=GTEST_LIBRARY-NOTFOUND
+
+//Path to a library.
+GTEST_LIBRARY_DEBUG:FILEPATH=GTEST_LIBRARY_DEBUG-NOTFOUND
+
+//Path to a library.
+GTEST_MAIN_LIBRARY:FILEPATH=GTEST_MAIN_LIBRARY-NOTFOUND
+
+//Path to a library.
+GTEST_MAIN_LIBRARY_DEBUG:FILEPATH=GTEST_MAIN_LIBRARY_DEBUG-NOTFOUND
+
+//lsb_release executable was found
+LSB_FOUND:BOOL=TRUE
+
+//Path to a program.
+LSB_RELEASE_EXECUTABLE:FILEPATH=/usr/bin/lsb_release
+
+//Path to a program.
+NOSETESTS:FILEPATH=/usr/bin/nosetests-2.7
+
+//Path to a program.
+PYTHON_EXECUTABLE:FILEPATH=/usr/bin/python
+
+//Specify specific Python version to use ('major.minor' or 'major')
+PYTHON_VERSION:STRING=
+
+//Value Computed by CMake
+Project_BINARY_DIR:STATIC=/home/roman/src/Firmware/build
+
+//Value Computed by CMake
+Project_SOURCE_DIR:STATIC=/home/roman/src/Firmware/src
+
+//Path to a library.
+RT_LIBRARY:FILEPATH=/usr/lib/x86_64-linux-gnu/librt.so
+
+//Enable debian style python package layout
+SETUPTOOLS_DEB_LAYOUT:BOOL=ON
+
+//LSB Distrib tag
+UBUNTU:BOOL=TRUE
+
+//LSB Distrib - codename tag
+UBUNTU_TRUSTY:BOOL=TRUE
+
+//Path to a file.
+_CATKIN_GTEST_INCLUDE:FILEPATH=/usr/include/gtest/gtest.h
+
+//Path to a file.
+_CATKIN_GTEST_SRC:FILEPATH=/usr/src/gtest/src/gtest.cc
+
+//The directory containing a CMake configuration file for catkin.
+catkin_DIR:PATH=/opt/ros/indigo/share/catkin/cmake
+
+//Value Computed by CMake
+gtest_BINARY_DIR:STATIC=/home/roman/src/Firmware/build/gtest
+
+//Dependencies for the target
+gtest_LIB_DEPENDS:STATIC=general;-lpthread;
+
+//Value Computed by CMake
+gtest_SOURCE_DIR:STATIC=/usr/src/gtest
+
+//Build gtest's sample programs.
+gtest_build_samples:BOOL=OFF
+
+//Build all of gtest's own tests.
+gtest_build_tests:BOOL=OFF
+
+//Disable uses of pthreads in gtest.
+gtest_disable_pthreads:BOOL=OFF
+
+//Use shared (DLL) run-time lib even when Google Test is built
+// as static lib.
+gtest_force_shared_crt:BOOL=OFF
+
+//Dependencies for the target
+gtest_main_LIB_DEPENDS:STATIC=general;-lpthread;general;gtest;
+
+
+########################
+# INTERNAL cache entries
+########################
+
+//catkin environment
+CATKIN_ENV:INTERNAL=/home/roman/src/Firmware/build/catkin_generated/env_cached.sh
+CATKIN_TEST_RESULTS_DIR:INTERNAL=/home/roman/src/Firmware/build/test_results
+//ADVANCED property for variable: CMAKE_AR
+CMAKE_AR-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_BUILD_TOOL
+CMAKE_BUILD_TOOL-ADVANCED:INTERNAL=1
+//What is the target build tool cmake is generating for.
+CMAKE_BUILD_TOOL:INTERNAL=/usr/bin/make
+//This is the directory where this CMakeCache.txt was created
+CMAKE_CACHEFILE_DIR:INTERNAL=/home/roman/src/Firmware/build
+//Major version of cmake used to create the current loaded cache
+CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2
+//Minor version of cmake used to create the current loaded cache
+CMAKE_CACHE_MINOR_VERSION:INTERNAL=8
+//Patch version of cmake used to create the current loaded cache
+CMAKE_CACHE_PATCH_VERSION:INTERNAL=12
+//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE
+CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1
+//Path to CMake executable.
+CMAKE_COMMAND:INTERNAL=/usr/bin/cmake
+//Path to cpack program executable.
+CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack
+//Path to ctest program executable.
+CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest
+//ADVANCED property for variable: CMAKE_CXX_COMPILER
+CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_CXX_FLAGS
+CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG
+CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL
+CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE
+CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO
+CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_C_COMPILER
+CMAKE_C_COMPILER-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_C_FLAGS
+CMAKE_C_FLAGS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG
+CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL
+CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE
+CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO
+CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
+//Executable file format
+CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF
+//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS
+CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG
+CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL
+CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE
+CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO
+CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS
+CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1
+//Name of generator.
+CMAKE_GENERATOR:INTERNAL=Unix Makefiles
+//Name of generator toolset.
+CMAKE_GENERATOR_TOOLSET:INTERNAL=
+//Have symbol pthread_create
+CMAKE_HAVE_LIBC_CREATE:INTERNAL=
+//Have library pthreads
+CMAKE_HAVE_PTHREADS_CREATE:INTERNAL=
+//Have library pthread
+CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1
+//Have include pthread.h
+CMAKE_HAVE_PTHREAD_H:INTERNAL=1
+//Start directory with the top level CMakeLists.txt file for this
+// project
+CMAKE_HOME_DIRECTORY:INTERNAL=/home/roman/src/Firmware/src
+//Install .so files without execute permission.
+CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1
+//ADVANCED property for variable: CMAKE_LINKER
+CMAKE_LINKER-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_MAKE_PROGRAM
+CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS
+CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG
+CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL
+CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE
+CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO
+CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_NM
+CMAKE_NM-ADVANCED:INTERNAL=1
+//number of local generators
+CMAKE_NUMBER_OF_LOCAL_GENERATORS:INTERNAL=2
+//ADVANCED property for variable: CMAKE_OBJCOPY
+CMAKE_OBJCOPY-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_OBJDUMP
+CMAKE_OBJDUMP-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_RANLIB
+CMAKE_RANLIB-ADVANCED:INTERNAL=1
+//Path to CMake installation.
+CMAKE_ROOT:INTERNAL=/usr/share/cmake-2.8
+//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS
+CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG
+CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL
+CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE
+CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO
+CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH
+CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_SKIP_RPATH
+CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS
+CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG
+CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL
+CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE
+CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO
+CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_STRIP
+CMAKE_STRIP-ADVANCED:INTERNAL=1
+//uname command
+CMAKE_UNAME:INTERNAL=/bin/uname
+//ADVANCED property for variable: CMAKE_USE_RELATIVE_PATHS
+CMAKE_USE_RELATIVE_PATHS-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE
+CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1
+//Details about finding PythonInterp
+FIND_PACKAGE_MESSAGE_DETAILS_PythonInterp:INTERNAL=[/usr/bin/python][v2.7.6()]
+//Details about finding Threads
+FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()]
+GTEST_FROM_SOURCE_FOUND:INTERNAL=TRUE
+GTEST_FROM_SOURCE_INCLUDE_DIRS:INTERNAL=/usr/include
+GTEST_FROM_SOURCE_LIBRARIES:INTERNAL=gtest
+GTEST_FROM_SOURCE_LIBRARY_DIRS:INTERNAL=/home/roman/src/Firmware/build/gtest
+GTEST_FROM_SOURCE_MAIN_LIBRARIES:INTERNAL=gtest_main
+//ADVANCED property for variable: GTEST_INCLUDE_DIR
+GTEST_INCLUDE_DIR-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: GTEST_LIBRARY
+GTEST_LIBRARY-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: GTEST_LIBRARY_DEBUG
+GTEST_LIBRARY_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: GTEST_MAIN_LIBRARY
+GTEST_MAIN_LIBRARY-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: GTEST_MAIN_LIBRARY_DEBUG
+GTEST_MAIN_LIBRARY_DEBUG-ADVANCED:INTERNAL=1
+//ADVANCED property for variable: PYTHON_EXECUTABLE
+PYTHON_EXECUTABLE-ADVANCED:INTERNAL=1
+//This needs to be in PYTHONPATH when 'setup.py install' is called.
+//  And it needs to match.  But setuptools won't tell us where
+// it will install things.
+PYTHON_INSTALL_DIR:INTERNAL=lib/python2.7/dist-packages
+
diff --git a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake
new file mode 100644
index 0000000000..83254ce491
--- /dev/null
+++ b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake
@@ -0,0 +1,56 @@
+set(CMAKE_C_COMPILER "/usr/bin/cc")
+set(CMAKE_C_COMPILER_ARG1 "")
+set(CMAKE_C_COMPILER_ID "GNU")
+set(CMAKE_C_COMPILER_VERSION "4.8.2")
+set(CMAKE_C_PLATFORM_ID "Linux")
+
+set(CMAKE_AR "/usr/bin/ar")
+set(CMAKE_RANLIB "/usr/bin/ranlib")
+set(CMAKE_LINKER "/usr/bin/ld")
+set(CMAKE_COMPILER_IS_GNUCC 1)
+set(CMAKE_C_COMPILER_LOADED 1)
+set(CMAKE_C_COMPILER_WORKS TRUE)
+set(CMAKE_C_ABI_COMPILED TRUE)
+set(CMAKE_COMPILER_IS_MINGW )
+set(CMAKE_COMPILER_IS_CYGWIN )
+if(CMAKE_COMPILER_IS_CYGWIN)
+  set(CYGWIN 1)
+  set(UNIX 1)
+endif()
+
+set(CMAKE_C_COMPILER_ENV_VAR "CC")
+
+if(CMAKE_COMPILER_IS_MINGW)
+  set(MINGW 1)
+endif()
+set(CMAKE_C_COMPILER_ID_RUN 1)
+set(CMAKE_C_SOURCE_FILE_EXTENSIONS c)
+set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC)
+set(CMAKE_C_LINKER_PREFERENCE 10)
+
+# Save compiler ABI information.
+set(CMAKE_C_SIZEOF_DATA_PTR "8")
+set(CMAKE_C_COMPILER_ABI "ELF")
+set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
+
+if(CMAKE_C_SIZEOF_DATA_PTR)
+  set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}")
+endif()
+
+if(CMAKE_C_COMPILER_ABI)
+  set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}")
+endif()
+
+if(CMAKE_C_LIBRARY_ARCHITECTURE)
+  set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
+endif()
+
+
+
+
+set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c")
+set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib")
+set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "")
+
+
+
diff --git a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake
new file mode 100644
index 0000000000..c4373d570c
--- /dev/null
+++ b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake
@@ -0,0 +1,57 @@
+set(CMAKE_CXX_COMPILER "/usr/bin/c++")
+set(CMAKE_CXX_COMPILER_ARG1 "")
+set(CMAKE_CXX_COMPILER_ID "GNU")
+set(CMAKE_CXX_COMPILER_VERSION "4.8.2")
+set(CMAKE_CXX_PLATFORM_ID "Linux")
+
+set(CMAKE_AR "/usr/bin/ar")
+set(CMAKE_RANLIB "/usr/bin/ranlib")
+set(CMAKE_LINKER "/usr/bin/ld")
+set(CMAKE_COMPILER_IS_GNUCXX 1)
+set(CMAKE_CXX_COMPILER_LOADED 1)
+set(CMAKE_CXX_COMPILER_WORKS TRUE)
+set(CMAKE_CXX_ABI_COMPILED TRUE)
+set(CMAKE_COMPILER_IS_MINGW )
+set(CMAKE_COMPILER_IS_CYGWIN )
+if(CMAKE_COMPILER_IS_CYGWIN)
+  set(CYGWIN 1)
+  set(UNIX 1)
+endif()
+
+set(CMAKE_CXX_COMPILER_ENV_VAR "CXX")
+
+if(CMAKE_COMPILER_IS_MINGW)
+  set(MINGW 1)
+endif()
+set(CMAKE_CXX_COMPILER_ID_RUN 1)
+set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC)
+set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP)
+set(CMAKE_CXX_LINKER_PREFERENCE 30)
+set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1)
+
+# Save compiler ABI information.
+set(CMAKE_CXX_SIZEOF_DATA_PTR "8")
+set(CMAKE_CXX_COMPILER_ABI "ELF")
+set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
+
+if(CMAKE_CXX_SIZEOF_DATA_PTR)
+  set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}")
+endif()
+
+if(CMAKE_CXX_COMPILER_ABI)
+  set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}")
+endif()
+
+if(CMAKE_CXX_LIBRARY_ARCHITECTURE)
+  set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
+endif()
+
+
+
+
+set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c")
+set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib")
+set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "")
+
+
+
diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin
new file mode 100755
index 0000000000000000000000000000000000000000..2f2ebe478334290d81ee498f150d9bab78baa7e7
GIT binary patch
literal 8547
zcmeHMeQaCR6+iaNheH#`?FyunQE#JLV9Fycp)(*x9M|b{>$F{)z_wy;o%kg&_@lF*
zOUufr$f#yP8T%0HDug=3ANhm*Ip{Vt(gjhZF{x}~LhB|Wx@>J}M-d&05Y3&=edoNq
zJUbH;5`XnqHP`pNbM86!<DC0(9*!sb{XQQs{P1<a+LKMaie&!<7+=a90KE`_7U+fR
zU?nsGK%=7B=p_j*XQ^o}1H`8RKm!0o{5B!tw>5EjE%9;57GjC|=D`-)<<l+p7cK$9
zihy!xYba=c4+VFsA6#~l9o<y299B`TljJ%{j>{8d7?<2X>>K_cr1l1>e_XD&&2UQO
zMQok1GWgq~hOB_yFD`qCk4u*O#*CFgJ3lEx#JG+0EjBI}sDE5i6>iF9hu8Jol*@GG
zvW3c?u04@;UF&-Ea#3HaY{K@Dcf;l(rHRL}gT`e!fLa6i{rvhPFK(P%(X%PF@1?gk
zKJ|y5ANiia^@06Cxn^9Cq5T2)!c|KG4GnESXn<b)hEt7t`r>b%yRRkom*1ZH@xIpE
zezxTKO{b>*`o~kxtQ|gi{;ltP^V<6!KD_VO2j362E`R5lqaBS$-~ID@G@G^Pa%O%Z
zmH@!Ag(QW-BK*Hxgnn!h`nOeIIv^6V*CiuO^iB``X+>WFy&n3AYQLj~{~-Wa3hmHK
z>xtLR<Eme6Fx70YGm{jL5uM`z;}@jYYbVa@GH8Q`sGZwM;3-940uhQQjL#E)H`zmO
z0tvEnE77kcfv+h3AoLL3K`uKyg3E3l2!X#v*-Dozkxyp}Oc^Ox#AvpV1(6sW5E-*%
zj%CZ1SsEOO<%$J!Fg=_zAzK(NigNZrvp6a;yQ~rjGn2N`AOse6t02T!zE~g?0z_hq
zC|j9qK~&0S#;q<}$XX!G0uqPIWftGKTWllcv0S=bHp{k6buHSEO!URXT79j4i``ou
zBj2L`(C6@<?H7p3HQ5&QqLVaWv+ef!LjOS@V|36uLSJJ&h_H+9<~JtJ1&qfUf7KEO
z<IyRr@$}6AU`)IcnC2F3$XlLm34>hyTuT_*SM`EijrDQ-c;;;>rCfa&>uB<s6H>}G
z=M&X8?lr3K7!&`vuyrtTd<s{Har_uAP~-S<TwkYOlu}+CZC|w>;OL#9oiJo{;d(SC
zW`jm`<^g|d`Y0+u!!D*?#!vF?_Er0lb`)#Oe{5DWsxwCQ_r|facN)GE#v8L0e`@+S
zw(y^<iz%b}sxAKx$?w0z*AfP(EFChcGpD~NrA)or`0qd%d>>3>|BQ({UTX>iSl2lw
zEFXF~9NY;;bw-{@o%ybmvV9etDN8A_jp{?^jB4fW<MEGC>tpeA#-a8X_T%zz|J{@?
z{xMTb7v<yBbo;9BBR@p%j#g##u0-|2=uo2iW^^!laCM|93`54jE-bevZ<twg(l~ZW
z!x=jrpOeyf^t^Q)%O|U6lhqH`S7lT#|C=!}?K5tDw{jLIaz`q9Pc#+1H!7w_M|I}^
zU>c|ST!5$Aw_vpZBsTYNxy|K9JGvt&o0uNX?uaM*Qvkk|+U?!6kDt3F<vswoc23G9
z#&jv=a{!Q(QoaHJFG(p|eW3OWgzv#^;M)`Ot!!-$Jn3r=;XZ<Agx4-fd86X6fpo+|
zet_$VfBce^XafX8{lSin?MogA?1wwAx&6kQum2*Fu}$2gQ*%<PSQZV24)|kjO}ks0
zacjnJ835QnC#8y+eZkNV{qbPO6IwhNKG+Zsc0S%{1iKG38NtXz^FXjS7mP%M-O*rY
zUohMk?C1-I`ho#v=kowCIVYvt?=IsqF5@yjYiy<STQ`*qKKFCkN+0-K*G6?dpRJ%e
z->+6uo$p82P`!ihQ+&_k@Bf*TMPxZd_a?scO*Po(RKDxX()TN<y?;@O=XC_0OA&mI
zWH@ITI^S^YReJ)=?>BfJLR?K{fX=Z@ztCvg!*;Vix`sOSIcfLj1o2C%^LSpM`Xqh-
ztibnJFLqNu4|vY)j195aZQ;(L;Yz`(gnRUezP4-4trb;WbAwF)(93)CR(crpvQ@In
zasHJpSY~M#=!K$X>e0SLmz5r)+E}5Y4_C6eOjkAosy3c3kAt4sTPW|%+oe^qzw9<k
z<!rIApdw13m&{xm1?bnVoCUf%lk1kb#{ykl2lP@=9r1N@T#S~|c~gvMO7k^aLZnNj
z^j=$nf8RG!Qf8#{*%8pkikA9gYq47#E|)<cDdzKL!QvtNR6zg1wZiL^-%0rWg#CxX
z>x%Iux<|7-zeDi*g#Ah~556BV)_CMC@;i$f#B-G!UU@t(eJEv-^|N0eA_Cuf+0WUZ
z7@Gl<PqRGx{VWk)qYh(Sz@IG7{&*MNcUYd|$P^jMaf9I<zk30c&$2xG=_DEaWfElj
znUBl+0aOg&I>&(%B*5eEwV&}Iy_>K+$DIo#&;4Zm-u~ll0lUib9LE9_NICAIJjQmf
z{1Y^;9CtbHM~J}joZ+4Ssz-h&#g&~N{S1De;_{~+d4898lH{NDOc={C4twNzpX#BG
za=d1E?LR{DJbwI+(o6Dq{dL2uf6^n*?|J;r$M1Ozul^T2@{{xqGD-R`vLJ!yk1>T;
z9oL<o_ho+f^YVM^uLD54OP=@b4th!R@_XxVk^Q{y;*A&){)F;>r}sdX=Y5OcW%>P;
z*C*@edHAPCKfi;_Vy9f|ox$>4zUz_aa|0{rWI+PUaf$PTOVqKS&K;d(Alu1cImSn1
zBg^yqTZ9C?{2q4zN&4B(87%LO-z?81KL4|BH|}Bm%t-K5Rh@mujnfpkx@qF@T@Y>L
z{%~AF|E#CKW0l?7CQsvD{r?LZ_?-RJ4GsLxKIw)AnzR48p+SSQueqT?qqASRp+S?g
z54oX1v$Marp+UgeH{8&m#n}(s(BKLu&by)2d5=YC(CWlxH#BH-;;kE6z4utv&ip&G
z8ybY1xaWojOPzS;h6c-=IOT>0pL61m8yYNk;))v@T<OFMH#E4)i34tEc-klbbVIB2
zu8z=Ph2xKIXf?Z{?FS9;KDy%q;GXNNV?W-b{P;faIfv^+zu=)qiSE6>-L2Z6x0C&2
zRMBh4^B}du_d&+b6@Tr#;A>~Se&N31gLc5@O9UrgP=7uCHx>U<IMibEGfpXb{rRoe
z&U;F5S?xOUp*<bY?U~;;MXy~aSE+%j?e7iL4##1}Es9>dem42+{D?Q-vBSri#)Wu5
z(QETKkNE67jyFH@6CZ9xwfUpNN^fo6<cOlz;$>=w_b0~di`aRR?BVkepIgpSf4zC7
zk5v1$c`Hf$e6C|O`LSQ~>z1+1Uq9b;wZHy&esvLgT(wi1pMsH+RkkXlqxuL4u`#wy
zBoo^Qg@F0=k+AY&1QSSQ;QWn?Bt)ht#&X5sbWUWfVyP_Bl|3+0%<syXmYLDl-TJk)
z^_f(z%D!p}vtX6>f_*v7RPy<~An2;9YAVmPfHgLdzTb?+iuqmHoLP$YCG-(HwUr*u
zf)M@Nq62Xe-@G2PSE^h58*j%>OPIFGl#60KUC8825bM9SIXaMtfpD{_L>mY(I1uAj
zHYB(7MU!GnfB*LQpcstyCF7_^<>fw67S5GxTQnhOr}DhXzTRGUzU)(H?YMLE$vZov
M_<v>a>@?ti0lyfbasU7T

literal 0
HcmV?d00001

diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin
new file mode 100755
index 0000000000000000000000000000000000000000..16c737f2604406275900604b68676d59840e9fbb
GIT binary patch
literal 8560
zcmeHMZ){w}5udx~e@E=QO9-t=()d~u)qwo4Lr4t~jnC(E-lgY+IyR(*%jWDm#~1F8
zd-o3PkU~^9sHYGbRYgss3Zbe}^MOiyqO?>csS!bls(}_nOOc`kS4jveh?H6t@m8~M
z=K1;ErJ_Qr`XRq4-rL!k+1a<dv%52oMC1Klj|UiD_$pv+S3{2?S$_e>mof)H4|IbM
zdSErIga!bpS2PPfB*EnzsphhY_%r~h1AuO?Md<cgJ#o2;__$;XvBdrsz!q9%)@QYF
z2@p~QltWvCeycs`-=%(V*-mzJQps{yMY(p8YbQA_Pmy6<^7wFU_<xA>4bk|x?6b_U
zOXNk|HEX8ucONP0YqG}0<$B`dlI6ZWYo^f7Pm3Ti4pM(t`Ys>P__(Ah+>*_VY*>Fw
zHr0{M<ja#ClieFSHmui61$~{e3H2lI=B>l3C;pDD^j($%s5OA?>sLPZ;2&=I&ZfP-
z=XbU~@$T5sWa2cg4;&B5H3Gol;Ql7~;&NY8U0uuf>YxYPaH_GN-sl?__xK`jK7aNH
z`<n0g(UPaXcJ}yRet-7Kbt7-T|JJv5tlIO)(S5%<^r63b`I#r*`{UnfCTpX^p83UC
z0szYvlN1tH;Qz@L=+9h%9#dm!h3=rWE*W<bz1>AWsOZ;2kBdI2^tV>=X8~X-1RzN3
ziPz0TYFsUFxY1f?Y3lfHqO%`h9HxHVcH+D)gBBPdJsU~jfTAyfAo&x<7l<Fo9@-}G
zWkvTx9no!MGb5w8yf%Ok_*<0BWYHA4WG2s)(NbBAW%3yiv7rHxN*B|+GbJ-!92$sZ
z3;Fa=awMAukxnJeBnW|pr38f7oh#%?qW}@xCQ4>1lNaSuI_1=!$!AOu={yogN+lNG
z@_-nmz9QLVsgy2RCe$=wb3E1?5$p7I`fX}(JaW$!{#meEt@Vy5ugLgtcf3OFa{+7J
z;J(1U!)T@TfqRI1LWCT2C%-X$vB@}C|Bf#N#)0Ez{kfX~z?goqX~xnhZ+qPr0=f22
zz7Pc3dO)tl`ZzY8KP{z{YhS@SZvFG8q?BFn$11PwG%9C|>32W4YbbW|IL^3n@&qn8
z<K#(PFXw(KrMxs2XzK%bZc}g<3>zJ|zKrQPzfqZe(3_Zf4m&`DE+t;TCV4v0wjXKF
zVU77u%xOkt)~Ni>IB{W<;W=fzHdpp0W`1o6ziwVi7?qbS`7<QH|IRL72%x-l*r?2&
zdss@Dc&UE1F9e>CW^jDQ^qq4JApmo=ZNl>57efABU{q%1sl@q*q?Ca+IA4-dqK?Y9
zFB+Bd>66h<u-Ajpi^h?_v-?9$JrMZqaZmIUrrhSBe3F<6w0#%(A$(7`BExsbDj$c3
zW0g0;L-9jv#~VU0Y#i#qa(n#7*{-*Z6PGodsdEGKQW^)|H&?TKymBF4`M9ql!*cmw
zjp-SWar=Aa3pj!ICc^iH6XBg<F*7!%Gyg|3IK3B}c#1uXRtZ3CYyY-8N}0Xs!k9=+
zn8kZLyAu{MIg+_I8t+d4cvfl~JLz1$cv;GQ0I+Ia$~gLHDdo=qATFhR5deN8rEK<q
z^8X0W-a+t820bgA8=DS$8iRN);F<sOWhrk`92Ss{Sj-QwhWN)XONlmsKiKbY-4a;x
zVAFosv|{5;x32jTl2Ip~xruoxm2V3BgO7S6Ee#JeH{wx?Z7BfQKQE>7fxZ6V_q|bn
z>*HF~A39VQ^|wD(Z}>YOZ7}@Z(~SfEo~*w+?C%Ww+k5??UVm$^KiKPUQg%KK0LSK~
zbndsSxQeT|ivM?n=>7&=68PTFWi!$FUe`i(zPDY^?etz+Np*f7t)O}<wevfV+y5~y
z3&?VW-c9`GJ631iQ~9kkNA1^;zBj4F`#J*erHJdOWY~8ex(BhZR%Zw-oKJXfLVTXe
z)Ad&WOn;f`Y&Z8u&rrKQFRk$$BYsJB{+=ID{b6eVUxDATZuHZA{chL2ov}F*xg*p*
zJW|e^<<NS)TVL1Fb$eM=yKb}y0D5UEXC_BLFPTNF9Oqw|yqPXefSxayX+7K<>oAkM
zskS>`)<?>jY^o!Z0#zGNmc~I(P322dIjb~_R?7qFVkuL|FRF+l=*4t4i30R%B5Q)K
zF7LXTo-{#M&jh_#P;UV}Jub$I$y{2Dr-}<TOF|@z#pIMF!N2#67L^&vTxJyX-33$q
zv3jvIj+9EEj}~&dbl&7o_L+eD2iFR(Q;w5x{Dk)pgVz<~26{)cJjWq8K4C>k7QpXA
zMwd(8q<v?SLcE7L;g-ky)yGm6xPRW)M~HAA3G+VZ{lwS`p!PJ&^S+-W!ZS2r^b7ct
z<#|8idlvUR%d;OjMuxKAV7R~EP5`xMS)TXl!(=dzpY3NpF7F3WK7i}&2ab^de}A|A
zjJ*^$VR`mDFOxivllym%A72+ZRF-EycA5m(?_m{vyIcNo`mXGE+3yF5!2X=!p8twV
zzLWe)C&_dF431B6`9qgH$0c@={7+pI#&V3KE_ptu>S&<suNiLppCWnwejG;$lKibK
zNO1Rm%q7q9JdX2mJdfe-|5=y(B*jN2ssBqXNZ|Qn9LJ}TBR0V2GROVg{O<az01$A<
z^SRwh(KI)|yZ#p0&*v__pb?=@DgO?|16iKWEso1_{FT=y_s{e2H&_20FPXzZIo3Oa
z<+*&%CC~Q;?x39o2`tAY&JQk8+kU!tw3C5sCxhh}{~{Y%p5t%bB<SXMZ3mFlzk8pt
zJo_&O%X5k0f9~6f`?!B*B)F=o&U?p+bL6<#4`5sn&l(;N`!(F3wfb*uv$XASDR=k(
zpP+%qK2M#{z-ynAPH3Rn=bsZA)Y<2n6B^Xp=amy0G}z~m6B;zy=Zg~>G}-5d6B_vJ
z^S}uWuCe{R6IzXUEJA~3+b=tzL5uBgozQB<X_cOZII|NP1Z}_Pga%7(f98Y+%WOa8
zga+5z{>KRomfL>C2@S5Z{e=@6e9ra*PH1@Br~T=KRwJ&C(BOJ|KRTgRor=~zsDlr2
zJ1zpAxt<#K<2%ZW@p;!hTqpW*7d=dL_xtT0rGLRr-XCL%Uj01}kskX!Ncgehubvl-
zj@BC2Da9WE41gl|9cKCC0ALst!ATjktCpURl%Az9?z8wAn7XX>-4M0(w5WkEt6D#+
zm7Z4Ubj|zCie9~b){`DSM;V(*5Bq1vc15pVR~e6$H*x1draYX1T#P3ay*j`1ln0NZ
z>ip5qmHz5{$}beX8n2QbKA#wWzJfjHJhk&m|D<ub^Gc0gw6i*I)$Fyt1K;Zy*Al<`
zd$tk3JKwa%Tl>4=JBjbuP7EqN)%htHEt(~>JT|6}f)HCGgCZW=J|qMzq?Lr36Qh_|
zDgh^Y93&x91+hC@7)fSD$}AL1B3Yh<(L!z_n>N!aeZ%cvSy!7$<*=-$rby?_;uKhq
z(^NT^n*u>kRaH|7sYR@jf#m(^$c`P6LT)0HO&7zxF@1Dm0>n4tT^lm_v4U^}^O4%E
zh{=%*2+=<n9*BzQ);>&vsX_B^d^_9yVD>9jDv0r9K7|>zzHe*|55yuMoNOvR2SN-D
zL|Dt___p3~Tx{#_-yR(jL*d?d6#G%xyib+8bEfPljc4Wi+!?{19%q*AGw1qvEDI?=
OEA9AS<^8PW;lBYd8IS4!

literal 0
HcmV?d00001

diff --git a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake
new file mode 100644
index 0000000000..0616b7c1ee
--- /dev/null
+++ b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake
@@ -0,0 +1,15 @@
+set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic")
+set(CMAKE_HOST_SYSTEM_NAME "Linux")
+set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic")
+set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64")
+
+
+
+set(CMAKE_SYSTEM "Linux-3.13.0-24-generic")
+set(CMAKE_SYSTEM_NAME "Linux")
+set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic")
+set(CMAKE_SYSTEM_PROCESSOR "x86_64")
+
+set(CMAKE_CROSSCOMPILING "FALSE")
+
+set(CMAKE_SYSTEM_LOADED 1)
diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c
new file mode 100644
index 0000000000..cba81d4a6b
--- /dev/null
+++ b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c
@@ -0,0 +1,389 @@
+#ifdef __cplusplus
+# error "A C++ compiler has been selected for C."
+#endif
+
+/* Version number components: V=Version, R=Revision, P=Patch
+   Version date components:   YYYY=Year, MM=Month,   DD=Day  */
+
+#if defined(__18CXX)
+# define ID_VOID_MAIN
+#endif
+
+#if defined(__INTEL_COMPILER) || defined(__ICC)
+# define COMPILER_ID "Intel"
+  /* __INTEL_COMPILER = VRP */
+# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
+# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
+# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER    % 10)
+# if defined(__INTEL_COMPILER_BUILD_DATE)
+  /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
+#  define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
+# endif
+
+#elif defined(__PATHCC__)
+# define COMPILER_ID "PathScale"
+# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
+# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
+# if defined(__PATHCC_PATCHLEVEL__)
+#  define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
+# endif
+
+#elif defined(__clang__)
+# define COMPILER_ID "Clang"
+# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
+# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
+# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
+
+#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
+# define COMPILER_ID "Embarcadero"
+# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
+# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
+# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__     & 0xFFFF)
+
+#elif defined(__BORLANDC__)
+# define COMPILER_ID "Borland"
+  /* __BORLANDC__ = 0xVRR */
+# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
+# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
+
+#elif defined(__WATCOMC__)
+# define COMPILER_ID "Watcom"
+  /* __WATCOMC__ = VVRR */
+# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
+# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100)
+
+#elif defined(__SUNPRO_C)
+# define COMPILER_ID "SunPro"
+# if __SUNPRO_C >= 0x5100
+   /* __SUNPRO_C = 0xVRRP */
+#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12)
+#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF)
+#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_C    & 0xF)
+# else
+   /* __SUNPRO_C = 0xVRP */
+#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8)
+#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF)
+#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_C    & 0xF)
+# endif
+
+#elif defined(__HP_cc)
+# define COMPILER_ID "HP"
+  /* __HP_cc = VVRRPP */
+# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000)
+# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100)
+# define COMPILER_VERSION_PATCH DEC(__HP_cc     % 100)
+
+#elif defined(__DECC)
+# define COMPILER_ID "Compaq"
+  /* __DECC_VER = VVRRTPPPP */
+# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000)
+# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000  % 100)
+# define COMPILER_VERSION_PATCH DEC(__DECC_VER         % 10000)
+
+#elif defined(__IBMC__)
+# if defined(__COMPILER_VER__)
+#  define COMPILER_ID "zOS"
+# else
+#  if __IBMC__ >= 800
+#   define COMPILER_ID "XL"
+#  else
+#   define COMPILER_ID "VisualAge"
+#  endif
+   /* __IBMC__ = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(__IBMC__/100)
+#  define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(__IBMC__    % 10)
+# endif
+
+#elif defined(__PGI)
+# define COMPILER_ID "PGI"
+# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
+# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
+# if defined(__PGIC_PATCHLEVEL__)
+#  define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
+# endif
+
+#elif defined(_CRAYC)
+# define COMPILER_ID "Cray"
+# define COMPILER_VERSION_MAJOR DEC(_RELEASE)
+# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
+
+#elif defined(__TI_COMPILER_VERSION__)
+# define COMPILER_ID "TI"
+  /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
+# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
+# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000   % 1000)
+# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__        % 1000)
+
+#elif defined(__TINYC__)
+# define COMPILER_ID "TinyCC"
+
+#elif defined(__SCO_VERSION__)
+# define COMPILER_ID "SCO"
+
+#elif defined(__GNUC__)
+# define COMPILER_ID "GNU"
+# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
+# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
+# if defined(__GNUC_PATCHLEVEL__)
+#  define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
+# endif
+
+#elif defined(_MSC_VER)
+# define COMPILER_ID "MSVC"
+  /* _MSC_VER = VVRR */
+# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
+# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
+# if defined(_MSC_FULL_VER)
+#  if _MSC_VER >= 1400
+    /* _MSC_FULL_VER = VVRRPPPPP */
+#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
+#  else
+    /* _MSC_FULL_VER = VVRRPPPP */
+#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
+#  endif
+# endif
+# if defined(_MSC_BUILD)
+#  define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
+# endif
+
+/* Analog VisualDSP++ >= 4.5.6 */
+#elif defined(__VISUALDSPVERSION__)
+# define COMPILER_ID "ADSP"
+  /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
+# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
+# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
+# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8  & 0xFF)
+
+/* Analog VisualDSP++ < 4.5.6 */
+#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
+# define COMPILER_ID "ADSP"
+
+/* IAR Systems compiler for embedded systems.
+   http://www.iar.com */
+#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC)
+# define COMPILER_ID "IAR"
+
+/* sdcc, the small devices C compiler for embedded systems,
+   http://sdcc.sourceforge.net  */
+#elif defined(SDCC)
+# define COMPILER_ID "SDCC"
+  /* SDCC = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(SDCC/100)
+#  define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(SDCC    % 10)
+
+#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION)
+# define COMPILER_ID "MIPSpro"
+# if defined(_SGI_COMPILER_VERSION)
+  /* _SGI_COMPILER_VERSION = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100)
+#  define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION    % 10)
+# else
+  /* _COMPILER_VERSION = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100)
+#  define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION    % 10)
+# endif
+
+/* This compiler is either not known or is too old to define an
+   identification macro.  Try to identify the platform and guess that
+   it is the native compiler.  */
+#elif defined(__sgi)
+# define COMPILER_ID "MIPSpro"
+
+#elif defined(__hpux) || defined(__hpua)
+# define COMPILER_ID "HP"
+
+#else /* unknown compiler */
+# define COMPILER_ID ""
+
+#endif
+
+/* Construct the string literal in pieces to prevent the source from
+   getting matched.  Store it in a pointer rather than an array
+   because some compilers will just produce instructions to fill the
+   array rather than assigning a pointer to a static array.  */
+char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
+
+/* Identify known platforms by name.  */
+#if defined(__linux) || defined(__linux__) || defined(linux)
+# define PLATFORM_ID "Linux"
+
+#elif defined(__CYGWIN__)
+# define PLATFORM_ID "Cygwin"
+
+#elif defined(__MINGW32__)
+# define PLATFORM_ID "MinGW"
+
+#elif defined(__APPLE__)
+# define PLATFORM_ID "Darwin"
+
+#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
+# define PLATFORM_ID "Windows"
+
+#elif defined(__FreeBSD__) || defined(__FreeBSD)
+# define PLATFORM_ID "FreeBSD"
+
+#elif defined(__NetBSD__) || defined(__NetBSD)
+# define PLATFORM_ID "NetBSD"
+
+#elif defined(__OpenBSD__) || defined(__OPENBSD)
+# define PLATFORM_ID "OpenBSD"
+
+#elif defined(__sun) || defined(sun)
+# define PLATFORM_ID "SunOS"
+
+#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
+# define PLATFORM_ID "AIX"
+
+#elif defined(__sgi) || defined(__sgi__) || defined(_SGI)
+# define PLATFORM_ID "IRIX"
+
+#elif defined(__hpux) || defined(__hpux__)
+# define PLATFORM_ID "HP-UX"
+
+#elif defined(__HAIKU__)
+# define PLATFORM_ID "Haiku"
+
+#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
+# define PLATFORM_ID "BeOS"
+
+#elif defined(__QNX__) || defined(__QNXNTO__)
+# define PLATFORM_ID "QNX"
+
+#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
+# define PLATFORM_ID "Tru64"
+
+#elif defined(__riscos) || defined(__riscos__)
+# define PLATFORM_ID "RISCos"
+
+#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
+# define PLATFORM_ID "SINIX"
+
+#elif defined(__UNIX_SV__)
+# define PLATFORM_ID "UNIX_SV"
+
+#elif defined(__bsdos__)
+# define PLATFORM_ID "BSDOS"
+
+#elif defined(_MPRAS) || defined(MPRAS)
+# define PLATFORM_ID "MP-RAS"
+
+#elif defined(__osf) || defined(__osf__)
+# define PLATFORM_ID "OSF1"
+
+#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
+# define PLATFORM_ID "SCO_SV"
+
+#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
+# define PLATFORM_ID "ULTRIX"
+
+#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
+# define PLATFORM_ID "Xenix"
+
+#else /* unknown platform */
+# define PLATFORM_ID ""
+
+#endif
+
+/* For windows compilers MSVC and Intel we can determine
+   the architecture of the compiler being used.  This is because
+   the compilers do not have flags that can change the architecture,
+   but rather depend on which compiler is being used
+*/
+#if defined(_WIN32) && defined(_MSC_VER)
+# if defined(_M_IA64)
+#  define ARCHITECTURE_ID "IA64"
+
+# elif defined(_M_X64) || defined(_M_AMD64)
+#  define ARCHITECTURE_ID "x64"
+
+# elif defined(_M_IX86)
+#  define ARCHITECTURE_ID "X86"
+
+# elif defined(_M_ARM)
+#  define ARCHITECTURE_ID "ARM"
+
+# elif defined(_M_MIPS)
+#  define ARCHITECTURE_ID "MIPS"
+
+# elif defined(_M_SH)
+#  define ARCHITECTURE_ID "SHx"
+
+# else /* unknown architecture */
+#  define ARCHITECTURE_ID ""
+# endif
+
+#else
+#  define ARCHITECTURE_ID ""
+#endif
+
+/* Convert integer to decimal digit literals.  */
+#define DEC(n)                   \
+  ('0' + (((n) / 10000000)%10)), \
+  ('0' + (((n) / 1000000)%10)),  \
+  ('0' + (((n) / 100000)%10)),   \
+  ('0' + (((n) / 10000)%10)),    \
+  ('0' + (((n) / 1000)%10)),     \
+  ('0' + (((n) / 100)%10)),      \
+  ('0' + (((n) / 10)%10)),       \
+  ('0' +  ((n) % 10))
+
+/* Convert integer to hex digit literals.  */
+#define HEX(n)             \
+  ('0' + ((n)>>28 & 0xF)), \
+  ('0' + ((n)>>24 & 0xF)), \
+  ('0' + ((n)>>20 & 0xF)), \
+  ('0' + ((n)>>16 & 0xF)), \
+  ('0' + ((n)>>12 & 0xF)), \
+  ('0' + ((n)>>8  & 0xF)), \
+  ('0' + ((n)>>4  & 0xF)), \
+  ('0' + ((n)     & 0xF))
+
+/* Construct a string literal encoding the version number components. */
+#ifdef COMPILER_VERSION_MAJOR
+char const info_version[] = {
+  'I', 'N', 'F', 'O', ':',
+  'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
+  COMPILER_VERSION_MAJOR,
+# ifdef COMPILER_VERSION_MINOR
+  '.', COMPILER_VERSION_MINOR,
+#  ifdef COMPILER_VERSION_PATCH
+   '.', COMPILER_VERSION_PATCH,
+#   ifdef COMPILER_VERSION_TWEAK
+    '.', COMPILER_VERSION_TWEAK,
+#   endif
+#  endif
+# endif
+  ']','\0'};
+#endif
+
+/* Construct the string literal in pieces to prevent the source from
+   getting matched.  Store it in a pointer rather than an array
+   because some compilers will just produce instructions to fill the
+   array rather than assigning a pointer to a static array.  */
+char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
+char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
+
+
+
+/*--------------------------------------------------------------------------*/
+
+#ifdef ID_VOID_MAIN
+void main() {}
+#else
+int main(int argc, char* argv[])
+{
+  int require = 0;
+  require += info_compiler[argc];
+  require += info_platform[argc];
+  require += info_arch[argc];
+#ifdef COMPILER_VERSION_MAJOR
+  require += info_version[argc];
+#endif
+  (void)argv;
+  return require;
+}
+#endif
diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out
new file mode 100755
index 0000000000000000000000000000000000000000..33a3d2b580160e3c32c235a17c6d2c6dc266b182
GIT binary patch
literal 8643
zcmeHMZ){sv6+iY%nsiBRr?il^uvlY7S+%^iSytRlqjp@U&y|ywG+8&b>5UURiJAWt
z`*}+SgF@IOE9+JXgtEbheL$i}d_<-2fwg2>KLAo%4QU9F%0#NRYo(%<DuOl_o%_yt
zd3kow;KM%U7scn@bMNoobN`+D@7u9NkJsY?h8Mm9DpVI%D(e^EWDBPN2tx?gLl_=}
zZLkgi>Xptym>4V<DQlLEluiSHIsgcHEk(#{mBjM%l#V4^h!V?N23u&!A2(PrECGT_
zfjqP|;IrZbzG3yjGE8<DB$<an<h2v8op>zg$uO2&Kdc-64^VysR6mycEHiA0X%UAO
z%oKi`Sg4X#y-bo_mfDug`|5(3BFZO1fKv5Sd8@U{ZK@wj3c_RA%*gJp$Fix8Y$jix
z?3fJg?%3U>mkN5PvI+CYwEO!8RY}~At<)}?0Mr=3`0u=RxufOa;FsR~)zp0H+TQP;
z?)%Zp$mRe41x*03u?FD_c8|J{&5Vqq*}Fjq{1zoMSu{m1naP6?<GDhfGy?V~;@wfv
zsdws60H_+Fjra$4dzQCm)??hGRLr8^8fVxy_Rs==)_|qQ;fj1l(4lvxH>Phk8Z-5=
z<{%iaoHy&QJq`fI^t+97%(Nh%=xh#x+}_n31b<r?<aP{CVC3~HQcAgf2Zpi8>+@2|
zUAN+uOUI4MhsN~ZZyy?nUpzlw9|q&%g<&Nw-l_}3wO>jp?~M7|_5qwb5*UU-qvKFu
z7>wyfpHW$O(K|GE4hulT?hL(yQSyqv?Ih~XVTjXTSk#Qlf>HUcapA^Z!!vJOS}c2q
z=6-FFFPnFUjLLf!|3l)ReEMfiL4b10pix=4_AM#p(0lbiYzl(s<2kIKG5z$j>w^I1
zgSH8q2Hy$#hQX*T$oZk`-;h%J+u(XhN{M+?UcPBm%2zJN?x|8|Hg`4!VJ3Fdm^D1}
z{<Gl!&3RAko-woep;h!7Yw1-6{<9~8>%x@7Ozhs!+}P>Jkw`^Go{3lPMh4@RKSl-;
zr?$^F24T=R)q(PG;*o`2e>N`M*RY4LZC;Ync;%M)pug>VXdfghHxiY*`zkUbH~qz!
zp7R)cJ}Te9jz2mSITjg;9FK^(u`!*~e>{i7;$|Zc4bO^ERE>CF&%q~03%L`SY`S=K
zf8XHH$}}tD#S7_TDO1QF-D#mqr;iYSbPidJ6WOFWRw(9<CeS;OGFcp*I64I2*`__U
zll=Ix`%-=r0A9N<WdZ=EmZba%0Nh-X@?8KpCZ%llfbtgz&rAK_nGATgH8(V#_A~_0
zkKh^+x-aDphv}$CtV|E^Fs1+Co|I?<_yRq?)&u^g7aLE)-mOpVc>LjqP>p$_kN(4b
zDZh@2h%fM}H@ab+H{0CMXvL+_rz}aC#`tbu;0<rg*ZR5^^94`U#eD6r)f>K@udXwE
zq3MQRUpVUvMSMFWzV>cku-n(#?F)4K8kL<t$GNj4rE|WoVGV0o!y4A`Y2rMc``#wW
z;5{nKW{NnYS?7}t6n?+n3U8q>pRcx2n9oOBDcnluDL&_M{6Cju0h7$qxye5BQF(mU
zIY;piP<|njcn(0|c>}STBm>W%3bfzmwO8#4V0phm$4U4cC7-17SwBo+wwudaPw6?l
zB(3^{DZQjHx94pNhbaEj0-s~uxJq{1UCkakkM@%sC%HcweKOcSI8x4=<zSZ{(mOkL
z?J29^u172dfL@x)naL5*OJ>oM6a1CQo9W^S(DMZ|tw*}!9cFTzLgV?eK2pwPQyrNU
zsL({RGy!^QDqot)S<)<8F|<)#5fDYti|K3<3G{U$Yl5E1XH3w|^rQ*;SSFtVy;w*k
z%_QjQ2{Beo=F(y!Ra_2P43R7rlT#LhzrQwGRAwY|nNiTk3#R(9O0jYrDV0DUE#z|P
zyy<T8{|h*;a18PI<@*!9SK)cZ;PJ-zB7hpp%;)<FzPGS$PnN;=4UAnbzDe`ZqzrMt
z?Sz|;d+fVX7Px$#@3W*}`+5HJJY)0$sJYI3_6O%kVNgQ!7g%@Z^Spe9?je}Z{)HU`
z`xAz{{f+~uxy*c?zaerk?5Eg%PRH_j0OcDv%>E%vd~ScY{fw9BK85-0kA{iQ_2ly1
z^~X~t_6hUZzfBUK{UP$v=ezl@Q@e6Kc|ABoeD?DUcmG#h{CV;>^RDt4d~e0_2QEI}
zTXI2{T^+_e##=5vuV3#|gRviHxb1(N_}qS1X#KlF{O>c7!2QWM<Kpu@AK(A+eILVJ
z{#h6Q4Ef<R)FF46NZ|fsoW~<D%>e|j*L)x7PVWx?9sv9fKCkDkbYJRD?+$-J_EUEQ
z-rOL9pOF7ox}Rk}uV;K;%lEQ8KDm4z2UlI?^L^bSR?0En8O&$-k&Dm!2`-?Wi3H}c
z#Qwn{YTHlyk#;h0H9q5CWFzzOx(#uN_*@RdH61`w`8>}V%y;{5=Cj0S0WRAKlgsCX
z1XoandG4$LZ6JrK<M95r!_gn?*KmH;Du1}m%5Bt@xx4)T1`Ry+dg_D*UVEK%LIcfS
z|D4dE&R*A?(4gL4ubj|uwAR>nY0zM=FHUICXs;ViXt3U151i280o%_zp+S@Fzn##a
z+4jp$Xt?Wlof8lm_-sGwga&@wKRTg7!1jAiXwYK&Gbc3IX!|KAH293|f1J=@lkHcW
z&|tIeFPzZev$h{_Lc>!4%`Yc3Xa%0PPH3>jo{vswRjZ;k59;6_I2~62*IZ8x`*C`B
zVFT=RwZBgKVV6EadiS~Rh{}K2PM#lQN?+ZcrzjsjCo<kt>8tw%Z{=&%3(Kjc|DDRG
z1!fzpb0p(WN?*GlYvr?`h#RZM!(Ek6b-!B;Uv2+J54L}GzqfnvooNH$YYBqgOvuA8
zhbu9l^wr};cyOs`tr{m|D*x(vlvVm_%uqh=aX7n*JwNpTw0K)#r)xa_0`-o0%KMEu
zrEi5WrRR0!614+97a`PLH~{=j*|P<nb>)AR(!2e}Ey_RS%KzV#53g&C2Cvor_&$YD
zcl5YN_113xpx3IeJD*2aN#CRLul9!t%m@5{4?T!Z^$YNv(zk-)vL8l^X2~p%jp?Hx
z#DQqPNW>2h2mz^dF+H9sndzc1b7C}G$frwCLnTD2AjY$Wkz`h+%tEmwlI2Oj7n^L_
zOsDkSd%oORJCVS{taUUM>AYE-0_!lFD(7-jAn0hTLITggE0RWglh3E4QF<7Pr<D0U
z{gK|7i1qCgf-1w`EBGl8`=0BI^v0tgoH6QU6~z9;!R|;x9PH^i92*b=k?uqcvr~_+
zqouMk8$^7dS1h;DKyMUVXdpR~O#>dKIkU7nR};tmfEx}wpKOJ25?4O$3Xv)m#6&Wm
z%BDfY4~mkR%H&14lukh=KUNU@pyf1FT^xus*4L_<O39CDAjC+iL^0NL@_`pbKXrOE
in=F;mB@kkuH_FY3b><<kqQ+KHDto@!Pyu{VgZ}`XxnsTn

literal 0
HcmV?d00001

diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
new file mode 100644
index 0000000000..e8220b26e5
--- /dev/null
+++ b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
@@ -0,0 +1,377 @@
+/* This source file must have a .cpp extension so that all C++ compilers
+   recognize the extension without flags.  Borland does not know .cxx for
+   example.  */
+#ifndef __cplusplus
+# error "A C compiler has been selected for C++."
+#endif
+
+/* Version number components: V=Version, R=Revision, P=Patch
+   Version date components:   YYYY=Year, MM=Month,   DD=Day  */
+
+#if defined(__COMO__)
+# define COMPILER_ID "Comeau"
+  /* __COMO_VERSION__ = VRR */
+# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100)
+# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100)
+
+#elif defined(__INTEL_COMPILER) || defined(__ICC)
+# define COMPILER_ID "Intel"
+  /* __INTEL_COMPILER = VRP */
+# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
+# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
+# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER    % 10)
+# if defined(__INTEL_COMPILER_BUILD_DATE)
+  /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
+#  define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
+# endif
+
+#elif defined(__PATHCC__)
+# define COMPILER_ID "PathScale"
+# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
+# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
+# if defined(__PATHCC_PATCHLEVEL__)
+#  define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
+# endif
+
+#elif defined(__clang__)
+# define COMPILER_ID "Clang"
+# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
+# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
+# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
+
+#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
+# define COMPILER_ID "Embarcadero"
+# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
+# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
+# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__     & 0xFFFF)
+
+#elif defined(__BORLANDC__)
+# define COMPILER_ID "Borland"
+  /* __BORLANDC__ = 0xVRR */
+# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
+# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
+
+#elif defined(__WATCOMC__)
+# define COMPILER_ID "Watcom"
+  /* __WATCOMC__ = VVRR */
+# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
+# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100)
+
+#elif defined(__SUNPRO_CC)
+# define COMPILER_ID "SunPro"
+# if __SUNPRO_CC >= 0x5100
+   /* __SUNPRO_CC = 0xVRRP */
+#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12)
+#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF)
+#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC    & 0xF)
+# else
+   /* __SUNPRO_CC = 0xVRP */
+#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8)
+#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF)
+#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC    & 0xF)
+# endif
+
+#elif defined(__HP_aCC)
+# define COMPILER_ID "HP"
+  /* __HP_aCC = VVRRPP */
+# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000)
+# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100)
+# define COMPILER_VERSION_PATCH DEC(__HP_aCC     % 100)
+
+#elif defined(__DECCXX)
+# define COMPILER_ID "Compaq"
+  /* __DECCXX_VER = VVRRTPPPP */
+# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000)
+# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000  % 100)
+# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER         % 10000)
+
+#elif defined(__IBMCPP__)
+# if defined(__COMPILER_VER__)
+#  define COMPILER_ID "zOS"
+# else
+#  if __IBMCPP__ >= 800
+#   define COMPILER_ID "XL"
+#  else
+#   define COMPILER_ID "VisualAge"
+#  endif
+   /* __IBMCPP__ = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
+#  define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(__IBMCPP__    % 10)
+# endif
+
+#elif defined(__PGI)
+# define COMPILER_ID "PGI"
+# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
+# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
+# if defined(__PGIC_PATCHLEVEL__)
+#  define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
+# endif
+
+#elif defined(_CRAYC)
+# define COMPILER_ID "Cray"
+# define COMPILER_VERSION_MAJOR DEC(_RELEASE)
+# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
+
+#elif defined(__TI_COMPILER_VERSION__)
+# define COMPILER_ID "TI"
+  /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
+# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
+# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000   % 1000)
+# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__        % 1000)
+
+#elif defined(__SCO_VERSION__)
+# define COMPILER_ID "SCO"
+
+#elif defined(__GNUC__)
+# define COMPILER_ID "GNU"
+# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
+# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
+# if defined(__GNUC_PATCHLEVEL__)
+#  define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
+# endif
+
+#elif defined(_MSC_VER)
+# define COMPILER_ID "MSVC"
+  /* _MSC_VER = VVRR */
+# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
+# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
+# if defined(_MSC_FULL_VER)
+#  if _MSC_VER >= 1400
+    /* _MSC_FULL_VER = VVRRPPPPP */
+#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
+#  else
+    /* _MSC_FULL_VER = VVRRPPPP */
+#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
+#  endif
+# endif
+# if defined(_MSC_BUILD)
+#  define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
+# endif
+
+/* Analog VisualDSP++ >= 4.5.6 */
+#elif defined(__VISUALDSPVERSION__)
+# define COMPILER_ID "ADSP"
+  /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
+# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
+# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
+# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8  & 0xFF)
+
+/* Analog VisualDSP++ < 4.5.6 */
+#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
+# define COMPILER_ID "ADSP"
+
+/* IAR Systems compiler for embedded systems.
+   http://www.iar.com */
+#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC)
+# define COMPILER_ID "IAR"
+
+#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION)
+# define COMPILER_ID "MIPSpro"
+# if defined(_SGI_COMPILER_VERSION)
+  /* _SGI_COMPILER_VERSION = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100)
+#  define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION    % 10)
+# else
+  /* _COMPILER_VERSION = VRP */
+#  define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100)
+#  define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10)
+#  define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION    % 10)
+# endif
+
+/* This compiler is either not known or is too old to define an
+   identification macro.  Try to identify the platform and guess that
+   it is the native compiler.  */
+#elif defined(__sgi)
+# define COMPILER_ID "MIPSpro"
+
+#elif defined(__hpux) || defined(__hpua)
+# define COMPILER_ID "HP"
+
+#else /* unknown compiler */
+# define COMPILER_ID ""
+
+#endif
+
+/* Construct the string literal in pieces to prevent the source from
+   getting matched.  Store it in a pointer rather than an array
+   because some compilers will just produce instructions to fill the
+   array rather than assigning a pointer to a static array.  */
+char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
+
+/* Identify known platforms by name.  */
+#if defined(__linux) || defined(__linux__) || defined(linux)
+# define PLATFORM_ID "Linux"
+
+#elif defined(__CYGWIN__)
+# define PLATFORM_ID "Cygwin"
+
+#elif defined(__MINGW32__)
+# define PLATFORM_ID "MinGW"
+
+#elif defined(__APPLE__)
+# define PLATFORM_ID "Darwin"
+
+#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
+# define PLATFORM_ID "Windows"
+
+#elif defined(__FreeBSD__) || defined(__FreeBSD)
+# define PLATFORM_ID "FreeBSD"
+
+#elif defined(__NetBSD__) || defined(__NetBSD)
+# define PLATFORM_ID "NetBSD"
+
+#elif defined(__OpenBSD__) || defined(__OPENBSD)
+# define PLATFORM_ID "OpenBSD"
+
+#elif defined(__sun) || defined(sun)
+# define PLATFORM_ID "SunOS"
+
+#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
+# define PLATFORM_ID "AIX"
+
+#elif defined(__sgi) || defined(__sgi__) || defined(_SGI)
+# define PLATFORM_ID "IRIX"
+
+#elif defined(__hpux) || defined(__hpux__)
+# define PLATFORM_ID "HP-UX"
+
+#elif defined(__HAIKU__)
+# define PLATFORM_ID "Haiku"
+
+#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
+# define PLATFORM_ID "BeOS"
+
+#elif defined(__QNX__) || defined(__QNXNTO__)
+# define PLATFORM_ID "QNX"
+
+#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
+# define PLATFORM_ID "Tru64"
+
+#elif defined(__riscos) || defined(__riscos__)
+# define PLATFORM_ID "RISCos"
+
+#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
+# define PLATFORM_ID "SINIX"
+
+#elif defined(__UNIX_SV__)
+# define PLATFORM_ID "UNIX_SV"
+
+#elif defined(__bsdos__)
+# define PLATFORM_ID "BSDOS"
+
+#elif defined(_MPRAS) || defined(MPRAS)
+# define PLATFORM_ID "MP-RAS"
+
+#elif defined(__osf) || defined(__osf__)
+# define PLATFORM_ID "OSF1"
+
+#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
+# define PLATFORM_ID "SCO_SV"
+
+#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
+# define PLATFORM_ID "ULTRIX"
+
+#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
+# define PLATFORM_ID "Xenix"
+
+#else /* unknown platform */
+# define PLATFORM_ID ""
+
+#endif
+
+/* For windows compilers MSVC and Intel we can determine
+   the architecture of the compiler being used.  This is because
+   the compilers do not have flags that can change the architecture,
+   but rather depend on which compiler is being used
+*/
+#if defined(_WIN32) && defined(_MSC_VER)
+# if defined(_M_IA64)
+#  define ARCHITECTURE_ID "IA64"
+
+# elif defined(_M_X64) || defined(_M_AMD64)
+#  define ARCHITECTURE_ID "x64"
+
+# elif defined(_M_IX86)
+#  define ARCHITECTURE_ID "X86"
+
+# elif defined(_M_ARM)
+#  define ARCHITECTURE_ID "ARM"
+
+# elif defined(_M_MIPS)
+#  define ARCHITECTURE_ID "MIPS"
+
+# elif defined(_M_SH)
+#  define ARCHITECTURE_ID "SHx"
+
+# else /* unknown architecture */
+#  define ARCHITECTURE_ID ""
+# endif
+
+#else
+#  define ARCHITECTURE_ID ""
+#endif
+
+/* Convert integer to decimal digit literals.  */
+#define DEC(n)                   \
+  ('0' + (((n) / 10000000)%10)), \
+  ('0' + (((n) / 1000000)%10)),  \
+  ('0' + (((n) / 100000)%10)),   \
+  ('0' + (((n) / 10000)%10)),    \
+  ('0' + (((n) / 1000)%10)),     \
+  ('0' + (((n) / 100)%10)),      \
+  ('0' + (((n) / 10)%10)),       \
+  ('0' +  ((n) % 10))
+
+/* Convert integer to hex digit literals.  */
+#define HEX(n)             \
+  ('0' + ((n)>>28 & 0xF)), \
+  ('0' + ((n)>>24 & 0xF)), \
+  ('0' + ((n)>>20 & 0xF)), \
+  ('0' + ((n)>>16 & 0xF)), \
+  ('0' + ((n)>>12 & 0xF)), \
+  ('0' + ((n)>>8  & 0xF)), \
+  ('0' + ((n)>>4  & 0xF)), \
+  ('0' + ((n)     & 0xF))
+
+/* Construct a string literal encoding the version number components. */
+#ifdef COMPILER_VERSION_MAJOR
+char const info_version[] = {
+  'I', 'N', 'F', 'O', ':',
+  'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
+  COMPILER_VERSION_MAJOR,
+# ifdef COMPILER_VERSION_MINOR
+  '.', COMPILER_VERSION_MINOR,
+#  ifdef COMPILER_VERSION_PATCH
+   '.', COMPILER_VERSION_PATCH,
+#   ifdef COMPILER_VERSION_TWEAK
+    '.', COMPILER_VERSION_TWEAK,
+#   endif
+#  endif
+# endif
+  ']','\0'};
+#endif
+
+/* Construct the string literal in pieces to prevent the source from
+   getting matched.  Store it in a pointer rather than an array
+   because some compilers will just produce instructions to fill the
+   array rather than assigning a pointer to a static array.  */
+char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
+char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
+
+
+
+/*--------------------------------------------------------------------------*/
+
+int main(int argc, char* argv[])
+{
+  int require = 0;
+  require += info_compiler[argc];
+  require += info_platform[argc];
+#ifdef COMPILER_VERSION_MAJOR
+  require += info_version[argc];
+#endif
+  (void)argv;
+  return require;
+}
diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out
new file mode 100755
index 0000000000000000000000000000000000000000..db35dbde0e2fe3ccc9c963b6592aa3f3f3eb7206
GIT binary patch
literal 8652
zcmeHMZ){sv6+iY%nsiBRw`(D7S!0zIWfgf#vMji(*4as&zEn<H+GOo$(;Fvt5^w$!
z+0R?D4X6m4FkQ!l5Gn|XA@LzdfW((lkYMRjtrCAkhJ-Z4kh+PCwrfSvN{OJ&Md!YA
zUS6JEH2APj`BlyFz4!d?IrrbW@80{xXsp}o@c_dMdqAbC!Ya%93vhBhYXF2H1ZyD-
z+h8lK0f1_iXCX`smWxz1%UaUY0H6v0LS8E)<h3lZ`~>N-WDilIy=Ab6mi$hQRl*V=
zs4|d;z6N|&dB8WMey|La9|lS0VG?;w#A_lR%XxB)CASaThX4DizJ6*S%e|HxwnSaT
z!38sczfCl%q}48yWV@AZOXhuc!Aub4Lm@y~hsfUD`sD_-k0mAH!E|c4qy52jqB)()
z7AKo0LmkZ>?Rp`nw<({ne$?I9H=rzWKQ>UmYy?nq0L%T~ZTiWcUqAPwue>7PY2JPP
zXE#4J@>S&W|NjMb0I;C~;XZprT}Y>fN6_sK5CVUTf*H@7A`?$#L5Q(TE=xHA_QiTS
zyF{DbraugzT7*90Kd}3=xFxj~%RWtJ7X8*d!?Cfa763E^tbClV$Y%r{`A&Uf`dY1V
zsybR91mlHSv-<Kw0ANgCsGVb`1^IAWeGuf1_WB_B8^a)XV0s)2uUwK+${jl~jV7<m
zODS8g_mti|VU*rArvGx|V1LiW+1cta7#GhURN~_GsxVyssg&~OsK0S9z_}xVAs8^4
z4+e(7m|pZ5rG;m_gLCK506KPa@HH%wm;8+<G4C9vSpWQ@W|S6;((A_gtB)C;dE?E+
zqIYoa=N9?*=FLH)^oGTMm-r_i{Xtz2ptyd(C@oz6f|PRbjp}dK1;O*d9JbGxe)Q<t
zAb`2ec46bdYeC--7^MX{KX~OiDW$&=t`ww{SV!se*Njr}(#7a)W%bmiwz?pkie59$
z7@m3mS@8dI))T#LoZ7U#Qod63pFJ5~6DG}5(c6P_qq@KG%UB;ojzmf_@_0|_R%D>3
z^oK})?DUSA+8_)Vr<+k8jy<r@`X}T39SukH@{T1bjTf$)+n66KU5%A)?JdcO-1xRJ
zJ?AlYy<fbF1AS~Tay&8^IS~<aqs#U_n8Rsst+v8cQ1GmnMdj$}>pt+vNG>yxN+<Kj
z_Vo=6T3Hk6xH+23XO6{ic~fRQKQex7a7866_~PkgzL3ggkF{86*Xc)yf3zXnvsK&O
zLUH`~9Vwp&fS2w_83TZ+B`LoT0B<izc>w@oQp$P{sCa?!JbMT{lL61x`kLBTJT(D~
zBe+L|?nt@Q;Th&5R_X)XPx^1&mJ)pcU!dF9u-{+zOzlZ{Y|HMQ58b~Vv$0N$(Z9VT
z<rA0@@daM=cCB0E?XRz?waOB>?w6!YVtJ=8@HKDL*YL6y^#xB?MSV>#RU5vR7uOiR
z&~#0&FP!#;BEFW0uc^})?DREs`U0K4TIJ^t(f=hWo$GxSt60S<R<VkY60>yfdzB=E
zzfoD%Q^Aa8oln+L`a-pp-b`seUu~r{pO3asx`ED9e9q(Ye=Nxys+^&7lYQnRdwkY8
zN9FfWeIb%~4nW{}1F?xD1J9od^u5h{ulgo{<?jtTPQu4YeUj{R{t%_vZ??CV^f|pG
zt@eaTUs9U;^9H3uRQ^$c&#`X2M}FM8n?H0Oy_aMY$$ed2j|7_rhKpIV7;M)=dRuer
zuA)k|K44`4=!L0_86O6{VCF43&c9MwGnt<NJ)1L=dZe?b*^G}-YAjpShl{CnqB)fS
zl^Ty1#z9X^WeZaoOPYDBgg!)9Bt#zcd@>zJ0{xmuo1mw%DHC)vIcb7En#!g?&*u_x
zGY)!kT#V-9nWPv`<d;(xL&Wp>_>{%q-%pO@l^gL)Y6SGLoT>g;R;(I_3kA?ea+ypr
zYr4Dq{{pTnoI^Z+`Tm6ORd`)7c)l?{383aO^Z9;)?=7s`lV$LI1EbZ&H)%bZR3UzE
zJK^TzH})+lb8Mg2`x(k$|9SoMI%D(!sI|^~jtA!`!yrYB7ua^@^SXST?je}Z@r45f
z#}kIT|4sm?wak28zaa`R9H-cS)?@h;fQk*A=J*gMKKH-df5x+PpTc~OM?=KtcCvkU
z`|;F@W5RrnZ<EC5c!+$A`ELHp)UVu5-VbJo&vBmN9{-YyKTq*y-esS`_f{;w;o|eX
zB^!F(HDJtReA~t6{p$jC7{_sj+y7UI&;56a_P<NS|2h*1Jf4gh7oYF>`2LUY`xtKf
zXI=amio-KBAUByv;PGS3;t`nE0D|{xz7KTkyVJh`0KbFJ`*{Q1m%8=c>9@##8g9Uw
z8$|F!^8ZZtv&`rHjPGmtUY6%4+vj=kp36Sp*DYeB9P^#Qe3tLK`20P=2AY^iU>-{x
zADp7L|MWf5L=N7K&-fSl$b7tRLmVVN+hMqt14y#Z>zu)Scl>5POMDh!+fJBlpA`wN
zq)PMJSpoWh9HN24`&->H9vs(jeO9tR)M(X~c2(}S|KFg2$KFq!(7<c&lTK)$+54Xp
z8dTZ)niCpS+xwLh8qU@V-(4Ei*!znU8r0hRh7%gBwf6%jG`Po(^G;|`XUA_RG^n@Z
zvJ)D9^}Egq2n~F89Cbniza1Z)&>&#PJts6+Z^ttyG}vIrDJL|z*N#6<Xt2?aD^6&z
z$&MFJXz(#R4mhFVDS+0O6B;xCuUjWH*le#yC$zF%(OL&p@ONB}D}Z~hr-J{uJiM?D
zT3r2)YXtL$T=@~occ1HysQQ=v<n=MC^2_`4G}XiBM8>z2e)+iIt$d|+{Z8ro0WZ=K
zZB!B3ucZHu(q9i}YAk)m-&B6(@vT(PzZG#q**w8JW>1Crut{}DdApjFe))XZ<H7f*
zb%3uZ2o67e9)3|=i6>Ql`TQw*a0_WDn?KL1`pehXmsEZ^zDM=A=jV^_;?FNU0PDRC
z(BhiszruXS`sMGCKdSr&2$MeVH-Dvmz-J_cx+4dGe=2`A!%<iLw@Kd}Pc$#uZGe!g
z{`Fq$pXGVWxX)|#KfYTb)ZIM*Z1+~~{|>C*Ti(x|chNtt>MxIz!{n#o@)Jh#X2C3u
zj_M;I#Qv^BBGz-bUkFGf^U1MP!A$0bnGqxDTsB#N3RyxVa$+o<8;++%!p!9hB3_&X
ze6dL<&16FF*!7vV%1Qzcv)0j6B(r9I3arCyqL|4{fuN(UN(npzuTbskjX#y_I(oE=
zo`!l7`pCoth|k4Z+fv!l96$Ad5Z#9&y-^YE+baY$k$<nKH34Gp6Md21o-Pp167}8+
zVqfe)XCx*Lbax+)_KW^VXDo`<sVCWyLQypaL{EROSnj$0-Y)F7{`hb@33$Nf%984|
zO&s^LZaD0G$Q8m#T>0QDM52%r<MC`FodnTyKorbGDl3YGWCBzt@GF_qQFWallGt9W
zX-boy*g%NkLV-%GN9O%ci$gROy3+ANAz1(+`g^;$8?ntiF;=wLGNr1=mpS+k*Z*B!

literal 0
HcmV?d00001

diff --git a/build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/CMakeFiles/CMakeDirectoryInformation.cmake
new file mode 100644
index 0000000000..b2cc9b7e7d
--- /dev/null
+++ b/build/CMakeFiles/CMakeDirectoryInformation.cmake
@@ -0,0 +1,16 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Relative path conversion top directories.
+SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/roman/src/Firmware/src")
+SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build")
+
+# Force unix paths in dependencies.
+SET(CMAKE_FORCE_UNIX_PATHS 1)
+
+
+# The C and CXX include file regular expressions for this directory.
+SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
+SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
+SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
+SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/build/CMakeFiles/CMakeError.log b/build/CMakeFiles/CMakeError.log
new file mode 100644
index 0000000000..9ab6f1e9a6
--- /dev/null
+++ b/build/CMakeFiles/CMakeError.log
@@ -0,0 +1,53 @@
+Determining if the pthread_create exist failed with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec1424100880/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec1424100880.dir/build.make CMakeFiles/cmTryCompileExec1424100880.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building C object CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o
+/usr/bin/cc    -o CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o   -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c
+Linking C executable cmTryCompileExec1424100880
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec1424100880.dir/link.txt --verbose=1
+/usr/bin/cc       CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o  -o cmTryCompileExec1424100880 -rdynamic 
+CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o: In function `main':
+CheckSymbolExists.c:(.text+0x16): undefined reference to `pthread_create'
+collect2: error: ld returned 1 exit status
+make[1]: *** [cmTryCompileExec1424100880] Error 1
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+make: *** [cmTryCompileExec1424100880/fast] Error 2
+
+File /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c:
+/* */
+#include <pthread.h>
+
+int main(int argc, char** argv)
+{
+  (void)argv;
+#ifndef pthread_create
+  return ((int*)(&pthread_create))[argc];
+#else
+  (void)argc;
+  return 0;
+#endif
+}
+
+Determining if the function pthread_create exists in the pthreads failed with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec26988121/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec26988121.dir/build.make CMakeFiles/cmTryCompileExec26988121.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building C object CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o
+/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create   -o CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o   -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c
+Linking C executable cmTryCompileExec26988121
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec26988121.dir/link.txt --verbose=1
+/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create    CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o  -o cmTryCompileExec26988121 -rdynamic -lpthreads 
+/usr/bin/ld: cannot find -lpthreads
+collect2: error: ld returned 1 exit status
+make[1]: *** [cmTryCompileExec26988121] Error 1
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+make: *** [cmTryCompileExec26988121/fast] Error 2
+
+
diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log
new file mode 100644
index 0000000000..a3757fc345
--- /dev/null
+++ b/build/CMakeFiles/CMakeOutput.log
@@ -0,0 +1,293 @@
+The system is: Linux - 3.13.0-24-generic - x86_64
+Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded.
+Compiler: /usr/bin/cc 
+Build flags: 
+Id flags: 
+
+The output was:
+0
+
+
+Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out"
+
+The C compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out"
+
+Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded.
+Compiler: /usr/bin/c++ 
+Build flags: 
+Id flags: 
+
+The output was:
+0
+
+
+Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out"
+
+The CXX compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out"
+
+Determining if the C compiler works passed with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec4189733644/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec4189733644.dir/build.make CMakeFiles/cmTryCompileExec4189733644.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building C object CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o
+/usr/bin/cc    -o CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o   -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCCompiler.c
+Linking C executable cmTryCompileExec4189733644
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4189733644.dir/link.txt --verbose=1
+/usr/bin/cc       CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o  -o cmTryCompileExec4189733644 -rdynamic 
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+
+
+Detecting C compiler ABI info compiled with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o
+/usr/bin/cc    -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o   -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c
+Linking C executable cmTryCompileExec2944435992
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1
+/usr/bin/cc     -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o  -o cmTryCompileExec2944435992 -rdynamic  
+Using built-in specs.
+COLLECT_GCC=/usr/bin/cc
+COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper
+Target: x86_64-linux-gnu
+Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu
+Thread model: posix
+gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) 
+COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/
+LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/
+COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64'
+ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+
+
+Parsed C implicit link information from above output:
+  link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)]
+  ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp]
+  ignore line: []
+  ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"]
+  ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build]
+  ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp']
+  ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1]
+  ignore line: [Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o]
+  ignore line: [/usr/bin/cc    -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o   -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c]
+  ignore line: [Linking C executable cmTryCompileExec2944435992]
+  ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1]
+  ignore line: [/usr/bin/cc     -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o  -o cmTryCompileExec2944435992 -rdynamic  ]
+  ignore line: [Using built-in specs.]
+  ignore line: [COLLECT_GCC=/usr/bin/cc]
+  ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper]
+  ignore line: [Target: x86_64-linux-gnu]
+  ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu]
+  ignore line: [Thread model: posix]
+  ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ]
+  ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/]
+  ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/]
+  ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64']
+  link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o]
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore
+    arg [--sysroot=/] ==> ignore
+    arg [--build-id] ==> ignore
+    arg [--eh-frame-hdr] ==> ignore
+    arg [-m] ==> ignore
+    arg [elf_x86_64] ==> ignore
+    arg [--hash-style=gnu] ==> ignore
+    arg [--as-needed] ==> ignore
+    arg [-export-dynamic] ==> ignore
+    arg [-dynamic-linker] ==> ignore
+    arg [/lib64/ld-linux-x86-64.so.2] ==> ignore
+    arg [-zrelro] ==> ignore
+    arg [-o] ==> ignore
+    arg [cmTryCompileExec2944435992] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8]
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu]
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib]
+    arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu]
+    arg [-L/lib/../lib] ==> dir [/lib/../lib]
+    arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu]
+    arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib]
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..]
+    arg [CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] ==> ignore
+    arg [-lgcc] ==> lib [gcc]
+    arg [--as-needed] ==> ignore
+    arg [-lgcc_s] ==> lib [gcc_s]
+    arg [--no-as-needed] ==> ignore
+    arg [-lc] ==> lib [c]
+    arg [-lgcc] ==> lib [gcc]
+    arg [--as-needed] ==> ignore
+    arg [-lgcc_s] ==> lib [gcc_s]
+    arg [--no-as-needed] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore
+  remove lib [gcc]
+  remove lib [gcc_s]
+  remove lib [gcc]
+  remove lib [gcc_s]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib]
+  collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu]
+  collapse library dir [/lib/../lib] ==> [/lib]
+  collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
+  collapse library dir [/usr/lib/../lib] ==> [/usr/lib]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib]
+  implicit libs: [c]
+  implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib]
+  implicit fwks: []
+
+
+Determining if the CXX compiler works passed with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec191173331/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec191173331.dir/build.make CMakeFiles/cmTryCompileExec191173331.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building CXX object CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o
+/usr/bin/c++     -o CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx
+Linking CXX executable cmTryCompileExec191173331
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec191173331.dir/link.txt --verbose=1
+/usr/bin/c++        CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o  -o cmTryCompileExec191173331 -rdynamic 
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+
+
+Detecting CXX compiler ABI info compiled with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o
+/usr/bin/c++     -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp
+Linking CXX executable cmTryCompileExec843003076
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1
+/usr/bin/c++      -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o  -o cmTryCompileExec843003076 -rdynamic  
+Using built-in specs.
+COLLECT_GCC=/usr/bin/c++
+COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper
+Target: x86_64-linux-gnu
+Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu
+Thread model: posix
+gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) 
+COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/
+LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/
+COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64'
+ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+
+
+Parsed CXX implicit link information from above output:
+  link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)]
+  ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp]
+  ignore line: []
+  ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"]
+  ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build]
+  ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp']
+  ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1]
+  ignore line: [Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o]
+  ignore line: [/usr/bin/c++     -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp]
+  ignore line: [Linking CXX executable cmTryCompileExec843003076]
+  ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1]
+  ignore line: [/usr/bin/c++      -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o  -o cmTryCompileExec843003076 -rdynamic  ]
+  ignore line: [Using built-in specs.]
+  ignore line: [COLLECT_GCC=/usr/bin/c++]
+  ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper]
+  ignore line: [Target: x86_64-linux-gnu]
+  ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu]
+  ignore line: [Thread model: posix]
+  ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ]
+  ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/]
+  ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/]
+  ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64']
+  link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o]
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore
+    arg [--sysroot=/] ==> ignore
+    arg [--build-id] ==> ignore
+    arg [--eh-frame-hdr] ==> ignore
+    arg [-m] ==> ignore
+    arg [elf_x86_64] ==> ignore
+    arg [--hash-style=gnu] ==> ignore
+    arg [--as-needed] ==> ignore
+    arg [-export-dynamic] ==> ignore
+    arg [-dynamic-linker] ==> ignore
+    arg [/lib64/ld-linux-x86-64.so.2] ==> ignore
+    arg [-zrelro] ==> ignore
+    arg [-o] ==> ignore
+    arg [cmTryCompileExec843003076] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8]
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu]
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib]
+    arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu]
+    arg [-L/lib/../lib] ==> dir [/lib/../lib]
+    arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu]
+    arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib]
+    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..]
+    arg [CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore
+    arg [-lstdc++] ==> lib [stdc++]
+    arg [-lm] ==> lib [m]
+    arg [-lgcc_s] ==> lib [gcc_s]
+    arg [-lgcc] ==> lib [gcc]
+    arg [-lc] ==> lib [c]
+    arg [-lgcc_s] ==> lib [gcc_s]
+    arg [-lgcc] ==> lib [gcc]
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore
+    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore
+  remove lib [gcc_s]
+  remove lib [gcc]
+  remove lib [gcc_s]
+  remove lib [gcc]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib]
+  collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu]
+  collapse library dir [/lib/../lib] ==> [/lib]
+  collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
+  collapse library dir [/usr/lib/../lib] ==> [/usr/lib]
+  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib]
+  implicit libs: [stdc++;m;c]
+  implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib]
+  implicit fwks: []
+
+
+Determining if files pthread.h exist passed with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec3897003384/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec3897003384.dir/build.make CMakeFiles/cmTryCompileExec3897003384.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building C object CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o
+/usr/bin/cc    -o CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o   -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckIncludeFiles.c
+Linking C executable cmTryCompileExec3897003384
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec3897003384.dir/link.txt --verbose=1
+/usr/bin/cc       CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o  -o cmTryCompileExec3897003384 -rdynamic 
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+
+
+Determining if the function pthread_create exists in the pthread passed with the following output:
+Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
+
+Run Build Command:/usr/bin/make "cmTryCompileExec4077710905/fast"
+/usr/bin/make -f CMakeFiles/cmTryCompileExec4077710905.dir/build.make CMakeFiles/cmTryCompileExec4077710905.dir/build
+make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
+Building C object CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o
+/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create   -o CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o   -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c
+Linking C executable cmTryCompileExec4077710905
+/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4077710905.dir/link.txt --verbose=1
+/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create    CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o  -o cmTryCompileExec4077710905 -rdynamic -lpthread 
+make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
+
+
diff --git a/build/CMakeFiles/CMakeRuleHashes.txt b/build/CMakeFiles/CMakeRuleHashes.txt
new file mode 100644
index 0000000000..b1bfe30c0e
--- /dev/null
+++ b/build/CMakeFiles/CMakeRuleHashes.txt
@@ -0,0 +1,5 @@
+# Hashes of file build rules.
+353bd50eb4c4b757d9a3d734d52d4f76 CMakeFiles/clean_test_results
+d83f452b18a5909d95cdb786c10abffb CMakeFiles/doxygen
+d83f452b18a5909d95cdb786c10abffb CMakeFiles/run_tests
+d83f452b18a5909d95cdb786c10abffb CMakeFiles/tests
diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake
new file mode 100644
index 0000000000..d232bacff9
--- /dev/null
+++ b/build/CMakeFiles/Makefile.cmake
@@ -0,0 +1,150 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# The generator used is:
+SET(CMAKE_DEPENDS_GENERATOR "Unix Makefiles")
+
+# The top level Makefile was generated from the following files:
+SET(CMAKE_MAKEFILE_DEPENDS
+  "CMakeCache.txt"
+  "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake"
+  "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake"
+  "CMakeFiles/2.8.12.2/CMakeSystem.cmake"
+  "catkin/catkin_generated/version/package.cmake"
+  "catkin_generated/order_packages.cmake"
+  "/home/roman/src/Firmware/src/CMakeLists.txt"
+  "/opt/ros/indigo/share/catkin/cmake/../package.xml"
+  "/opt/ros/indigo/share/catkin/cmake/all.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/assert.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/atomic_configure_file.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkinConfig-version.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_add_env_hooks.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_destinations.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_generate_environment.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_install_python.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_libraries.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_metapackage.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_package.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_package_xml.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_python_setup.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/debug_message.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/em/order_packages.cmake.em"
+  "/opt/ros/indigo/share/catkin/cmake/em_expand.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/empy.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in"
+  "/opt/ros/indigo/share/catkin/cmake/find_program_required.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/interrogate_setup_dot_py.py"
+  "/opt/ros/indigo/share/catkin/cmake/legacy.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/list_append_deduplicate.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/list_append_unique.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/list_insert_in_workspace_order.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/platform/lsb.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/platform/ubuntu.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/platform/windows.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/python.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/safe_execute_process.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/stamp.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/string_starts_with.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/templates/_setup_util.py.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/env.sh.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/generate_cached_setup.py.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/order_packages.context.py.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/rosinstall.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/setup.bash.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/setup.sh.in"
+  "/opt/ros/indigo/share/catkin/cmake/templates/setup.zsh.in"
+  "/opt/ros/indigo/share/catkin/cmake/test/catkin_download_test_data.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/test/gtest.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/test/nosetests.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/test/tests.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/tools/doxygen.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/tools/libraries.cmake"
+  "/opt/ros/indigo/share/catkin/cmake/tools/rt.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeCCompiler.cmake.in"
+  "/usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c"
+  "/usr/share/cmake-2.8/Modules/CMakeCInformation.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeCXXCompiler.cmake.in"
+  "/usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp"
+  "/usr/share/cmake-2.8/Modules/CMakeCXXInformation.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeClDeps.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeCommonLanguageInclude.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeConfigurableFile.in"
+  "/usr/share/cmake-2.8/Modules/CMakeDetermineCCompiler.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeDetermineCXXCompiler.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeDetermineCompiler.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerABI.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerId.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeDetermineSystem.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeFindBinUtils.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeGenericSystem.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeParseArguments.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeParseImplicitLinkInfo.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeSystem.cmake.in"
+  "/usr/share/cmake-2.8/Modules/CMakeSystemSpecificInformation.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeTestCCompiler.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeTestCXXCompiler.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeTestCompilerCommon.cmake"
+  "/usr/share/cmake-2.8/Modules/CMakeUnixFindMake.cmake"
+  "/usr/share/cmake-2.8/Modules/CheckFunctionExists.c"
+  "/usr/share/cmake-2.8/Modules/CheckIncludeFiles.cmake"
+  "/usr/share/cmake-2.8/Modules/CheckLibraryExists.cmake"
+  "/usr/share/cmake-2.8/Modules/CheckSymbolExists.cmake"
+  "/usr/share/cmake-2.8/Modules/Compiler/GNU-C.cmake"
+  "/usr/share/cmake-2.8/Modules/Compiler/GNU-CXX.cmake"
+  "/usr/share/cmake-2.8/Modules/Compiler/GNU.cmake"
+  "/usr/share/cmake-2.8/Modules/FindGTest.cmake"
+  "/usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake"
+  "/usr/share/cmake-2.8/Modules/FindPackageMessage.cmake"
+  "/usr/share/cmake-2.8/Modules/FindPythonInterp.cmake"
+  "/usr/share/cmake-2.8/Modules/FindThreads.cmake"
+  "/usr/share/cmake-2.8/Modules/MultiArchCross.cmake"
+  "/usr/share/cmake-2.8/Modules/Platform/Linux-CXX.cmake"
+  "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-C.cmake"
+  "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-CXX.cmake"
+  "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU.cmake"
+  "/usr/share/cmake-2.8/Modules/Platform/Linux.cmake"
+  "/usr/share/cmake-2.8/Modules/Platform/UnixPaths.cmake"
+  "/usr/src/gtest/CMakeLists.txt"
+  "/usr/src/gtest/cmake/internal_utils.cmake"
+  )
+
+# The corresponding makefile is:
+SET(CMAKE_MAKEFILE_OUTPUTS
+  "Makefile"
+  "CMakeFiles/cmake.check_cache"
+  )
+
+# Byproducts of CMake generate step:
+SET(CMAKE_MAKEFILE_PRODUCTS
+  "CMakeFiles/2.8.12.2/CMakeSystem.cmake"
+  "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake"
+  "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake"
+  "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake"
+  "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake"
+  "catkin_generated/stamps/Project/package.xml.stamp"
+  "catkin_generated/installspace/_setup_util.py"
+  "catkin_generated/installspace/env.sh"
+  "catkin_generated/installspace/setup.bash"
+  "catkin_generated/installspace/setup.sh"
+  "catkin_generated/installspace/setup.zsh"
+  "catkin_generated/installspace/.rosinstall"
+  "catkin_generated/generate_cached_setup.py"
+  "catkin_generated/env_cached.sh"
+  "catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp"
+  "catkin_generated/order_packages.py"
+  "catkin_generated/stamps/Project/order_packages.cmake.em.stamp"
+  "CMakeFiles/CMakeDirectoryInformation.cmake"
+  "gtest/CMakeFiles/CMakeDirectoryInformation.cmake"
+  )
+
+# Dependency information for all targets:
+SET(CMAKE_DEPEND_INFO_FILES
+  "CMakeFiles/clean_test_results.dir/DependInfo.cmake"
+  "CMakeFiles/doxygen.dir/DependInfo.cmake"
+  "CMakeFiles/run_tests.dir/DependInfo.cmake"
+  "CMakeFiles/tests.dir/DependInfo.cmake"
+  "gtest/CMakeFiles/gtest.dir/DependInfo.cmake"
+  "gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake"
+  )
diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2
new file mode 100644
index 0000000000..2a11fe8ef9
--- /dev/null
+++ b/build/CMakeFiles/Makefile2
@@ -0,0 +1,266 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Default target executed when no arguments are given to make.
+default_target: all
+.PHONY : default_target
+
+# The main recursive all target
+all:
+.PHONY : all
+
+# The main recursive preinstall target
+preinstall:
+.PHONY : preinstall
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+#=============================================================================
+# Target rules for target CMakeFiles/clean_test_results.dir
+
+# All Build rule for target.
+CMakeFiles/clean_test_results.dir/all:
+	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/depend
+	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
+	@echo "Built target clean_test_results"
+.PHONY : CMakeFiles/clean_test_results.dir/all
+
+# Build rule for subdir invocation for target.
+CMakeFiles/clean_test_results.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/clean_test_results.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : CMakeFiles/clean_test_results.dir/rule
+
+# Convenience name for target.
+clean_test_results: CMakeFiles/clean_test_results.dir/rule
+.PHONY : clean_test_results
+
+# clean rule for target.
+CMakeFiles/clean_test_results.dir/clean:
+	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/clean
+.PHONY : CMakeFiles/clean_test_results.dir/clean
+
+# clean rule for target.
+clean: CMakeFiles/clean_test_results.dir/clean
+.PHONY : clean
+
+#=============================================================================
+# Target rules for target CMakeFiles/doxygen.dir
+
+# All Build rule for target.
+CMakeFiles/doxygen.dir/all:
+	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/depend
+	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
+	@echo "Built target doxygen"
+.PHONY : CMakeFiles/doxygen.dir/all
+
+# Build rule for subdir invocation for target.
+CMakeFiles/doxygen.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/doxygen.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : CMakeFiles/doxygen.dir/rule
+
+# Convenience name for target.
+doxygen: CMakeFiles/doxygen.dir/rule
+.PHONY : doxygen
+
+# clean rule for target.
+CMakeFiles/doxygen.dir/clean:
+	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/clean
+.PHONY : CMakeFiles/doxygen.dir/clean
+
+# clean rule for target.
+clean: CMakeFiles/doxygen.dir/clean
+.PHONY : clean
+
+#=============================================================================
+# Target rules for target CMakeFiles/run_tests.dir
+
+# All Build rule for target.
+CMakeFiles/run_tests.dir/all:
+	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/depend
+	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
+	@echo "Built target run_tests"
+.PHONY : CMakeFiles/run_tests.dir/all
+
+# Build rule for subdir invocation for target.
+CMakeFiles/run_tests.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/run_tests.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : CMakeFiles/run_tests.dir/rule
+
+# Convenience name for target.
+run_tests: CMakeFiles/run_tests.dir/rule
+.PHONY : run_tests
+
+# clean rule for target.
+CMakeFiles/run_tests.dir/clean:
+	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/clean
+.PHONY : CMakeFiles/run_tests.dir/clean
+
+# clean rule for target.
+clean: CMakeFiles/run_tests.dir/clean
+.PHONY : clean
+
+#=============================================================================
+# Target rules for target CMakeFiles/tests.dir
+
+# All Build rule for target.
+CMakeFiles/tests.dir/all:
+	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/depend
+	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
+	@echo "Built target tests"
+.PHONY : CMakeFiles/tests.dir/all
+
+# Build rule for subdir invocation for target.
+CMakeFiles/tests.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/tests.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : CMakeFiles/tests.dir/rule
+
+# Convenience name for target.
+tests: CMakeFiles/tests.dir/rule
+.PHONY : tests
+
+# clean rule for target.
+CMakeFiles/tests.dir/clean:
+	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/clean
+.PHONY : CMakeFiles/tests.dir/clean
+
+# clean rule for target.
+clean: CMakeFiles/tests.dir/clean
+.PHONY : clean
+
+#=============================================================================
+# Directory level rules for directory gtest
+
+# Convenience name for "all" pass in the directory.
+gtest/all:
+.PHONY : gtest/all
+
+# Convenience name for "clean" pass in the directory.
+gtest/clean: gtest/CMakeFiles/gtest.dir/clean
+gtest/clean: gtest/CMakeFiles/gtest_main.dir/clean
+.PHONY : gtest/clean
+
+# Convenience name for "preinstall" pass in the directory.
+gtest/preinstall:
+.PHONY : gtest/preinstall
+
+#=============================================================================
+# Target rules for target gtest/CMakeFiles/gtest.dir
+
+# All Build rule for target.
+gtest/CMakeFiles/gtest.dir/all:
+	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/depend
+	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles  1
+	@echo "Built target gtest"
+.PHONY : gtest/CMakeFiles/gtest.dir/all
+
+# Build rule for subdir invocation for target.
+gtest/CMakeFiles/gtest.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 1
+	$(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : gtest/CMakeFiles/gtest.dir/rule
+
+# Convenience name for target.
+gtest: gtest/CMakeFiles/gtest.dir/rule
+.PHONY : gtest
+
+# clean rule for target.
+gtest/CMakeFiles/gtest.dir/clean:
+	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/clean
+.PHONY : gtest/CMakeFiles/gtest.dir/clean
+
+# clean rule for target.
+clean: gtest/CMakeFiles/gtest.dir/clean
+.PHONY : clean
+
+#=============================================================================
+# Target rules for target gtest/CMakeFiles/gtest_main.dir
+
+# All Build rule for target.
+gtest/CMakeFiles/gtest_main.dir/all: gtest/CMakeFiles/gtest.dir/all
+	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/depend
+	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles  2
+	@echo "Built target gtest_main"
+.PHONY : gtest/CMakeFiles/gtest_main.dir/all
+
+# Build rule for subdir invocation for target.
+gtest/CMakeFiles/gtest_main.dir/rule: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 2
+	$(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : gtest/CMakeFiles/gtest_main.dir/rule
+
+# Convenience name for target.
+gtest_main: gtest/CMakeFiles/gtest_main.dir/rule
+.PHONY : gtest_main
+
+# clean rule for target.
+gtest/CMakeFiles/gtest_main.dir/clean:
+	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/clean
+.PHONY : gtest/CMakeFiles/gtest_main.dir/clean
+
+# clean rule for target.
+clean: gtest/CMakeFiles/gtest_main.dir/clean
+.PHONY : clean
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+
diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt
new file mode 100644
index 0000000000..552e0c540a
--- /dev/null
+++ b/build/CMakeFiles/TargetDirectories.txt
@@ -0,0 +1,6 @@
+/home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir
+/home/roman/src/Firmware/build/CMakeFiles/doxygen.dir
+/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir
+/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir
+/home/roman/src/Firmware/build/CMakeFiles/run_tests.dir
+/home/roman/src/Firmware/build/CMakeFiles/tests.dir
diff --git a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake
new file mode 100644
index 0000000000..7aff3a53c3
--- /dev/null
+++ b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake
@@ -0,0 +1,20 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+  )
+# The set of files for implicit dependencies of each language:
+
+# Preprocessor definitions for this target.
+SET(CMAKE_TARGET_DEFINITIONS
+  "ROS_BUILD_SHARED_LIBS=1"
+  )
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+  )
+
+# The include file search paths:
+SET(CMAKE_C_TARGET_INCLUDE_PATH
+  )
+SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/clean_test_results.dir/build.make b/build/CMakeFiles/clean_test_results.dir/build.make
new file mode 100644
index 0000000000..b3776d0df4
--- /dev/null
+++ b/build/CMakeFiles/clean_test_results.dir/build.make
@@ -0,0 +1,66 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+# Utility rule file for clean_test_results.
+
+# Include the progress variables for this target.
+include CMakeFiles/clean_test_results.dir/progress.make
+
+CMakeFiles/clean_test_results:
+	/usr/bin/cmake -E remove_directory /home/roman/src/Firmware/build/test_results
+
+clean_test_results: CMakeFiles/clean_test_results
+clean_test_results: CMakeFiles/clean_test_results.dir/build.make
+.PHONY : clean_test_results
+
+# Rule to build all files generated by this target.
+CMakeFiles/clean_test_results.dir/build: clean_test_results
+.PHONY : CMakeFiles/clean_test_results.dir/build
+
+CMakeFiles/clean_test_results.dir/clean:
+	$(CMAKE_COMMAND) -P CMakeFiles/clean_test_results.dir/cmake_clean.cmake
+.PHONY : CMakeFiles/clean_test_results.dir/clean
+
+CMakeFiles/clean_test_results.dir/depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : CMakeFiles/clean_test_results.dir/depend
+
diff --git a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake
new file mode 100644
index 0000000000..46c1cb3380
--- /dev/null
+++ b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake
@@ -0,0 +1,8 @@
+FILE(REMOVE_RECURSE
+  "CMakeFiles/clean_test_results"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang)
+  INCLUDE(CMakeFiles/clean_test_results.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/build/CMakeFiles/clean_test_results.dir/progress.make b/build/CMakeFiles/clean_test_results.dir/progress.make
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/build/CMakeFiles/clean_test_results.dir/progress.make
@@ -0,0 +1 @@
+
diff --git a/build/CMakeFiles/cmake.check_cache b/build/CMakeFiles/cmake.check_cache
new file mode 100644
index 0000000000..3dccd73172
--- /dev/null
+++ b/build/CMakeFiles/cmake.check_cache
@@ -0,0 +1 @@
+# This file is generated by cmake for dependency checking of the CMakeCache.txt file
diff --git a/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/build/CMakeFiles/doxygen.dir/DependInfo.cmake
new file mode 100644
index 0000000000..7aff3a53c3
--- /dev/null
+++ b/build/CMakeFiles/doxygen.dir/DependInfo.cmake
@@ -0,0 +1,20 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+  )
+# The set of files for implicit dependencies of each language:
+
+# Preprocessor definitions for this target.
+SET(CMAKE_TARGET_DEFINITIONS
+  "ROS_BUILD_SHARED_LIBS=1"
+  )
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+  )
+
+# The include file search paths:
+SET(CMAKE_C_TARGET_INCLUDE_PATH
+  )
+SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/doxygen.dir/build.make b/build/CMakeFiles/doxygen.dir/build.make
new file mode 100644
index 0000000000..f5cc02bb39
--- /dev/null
+++ b/build/CMakeFiles/doxygen.dir/build.make
@@ -0,0 +1,65 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+# Utility rule file for doxygen.
+
+# Include the progress variables for this target.
+include CMakeFiles/doxygen.dir/progress.make
+
+CMakeFiles/doxygen:
+
+doxygen: CMakeFiles/doxygen
+doxygen: CMakeFiles/doxygen.dir/build.make
+.PHONY : doxygen
+
+# Rule to build all files generated by this target.
+CMakeFiles/doxygen.dir/build: doxygen
+.PHONY : CMakeFiles/doxygen.dir/build
+
+CMakeFiles/doxygen.dir/clean:
+	$(CMAKE_COMMAND) -P CMakeFiles/doxygen.dir/cmake_clean.cmake
+.PHONY : CMakeFiles/doxygen.dir/clean
+
+CMakeFiles/doxygen.dir/depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/doxygen.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : CMakeFiles/doxygen.dir/depend
+
diff --git a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake
new file mode 100644
index 0000000000..3cf72d90fa
--- /dev/null
+++ b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake
@@ -0,0 +1,8 @@
+FILE(REMOVE_RECURSE
+  "CMakeFiles/doxygen"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang)
+  INCLUDE(CMakeFiles/doxygen.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/build/CMakeFiles/doxygen.dir/progress.make b/build/CMakeFiles/doxygen.dir/progress.make
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/build/CMakeFiles/doxygen.dir/progress.make
@@ -0,0 +1 @@
+
diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks
new file mode 100644
index 0000000000..573541ac97
--- /dev/null
+++ b/build/CMakeFiles/progress.marks
@@ -0,0 +1 @@
+0
diff --git a/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/build/CMakeFiles/run_tests.dir/DependInfo.cmake
new file mode 100644
index 0000000000..7aff3a53c3
--- /dev/null
+++ b/build/CMakeFiles/run_tests.dir/DependInfo.cmake
@@ -0,0 +1,20 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+  )
+# The set of files for implicit dependencies of each language:
+
+# Preprocessor definitions for this target.
+SET(CMAKE_TARGET_DEFINITIONS
+  "ROS_BUILD_SHARED_LIBS=1"
+  )
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+  )
+
+# The include file search paths:
+SET(CMAKE_C_TARGET_INCLUDE_PATH
+  )
+SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/run_tests.dir/build.make b/build/CMakeFiles/run_tests.dir/build.make
new file mode 100644
index 0000000000..3907c75732
--- /dev/null
+++ b/build/CMakeFiles/run_tests.dir/build.make
@@ -0,0 +1,65 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+# Utility rule file for run_tests.
+
+# Include the progress variables for this target.
+include CMakeFiles/run_tests.dir/progress.make
+
+CMakeFiles/run_tests:
+
+run_tests: CMakeFiles/run_tests
+run_tests: CMakeFiles/run_tests.dir/build.make
+.PHONY : run_tests
+
+# Rule to build all files generated by this target.
+CMakeFiles/run_tests.dir/build: run_tests
+.PHONY : CMakeFiles/run_tests.dir/build
+
+CMakeFiles/run_tests.dir/clean:
+	$(CMAKE_COMMAND) -P CMakeFiles/run_tests.dir/cmake_clean.cmake
+.PHONY : CMakeFiles/run_tests.dir/clean
+
+CMakeFiles/run_tests.dir/depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/run_tests.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : CMakeFiles/run_tests.dir/depend
+
diff --git a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake
new file mode 100644
index 0000000000..45a3e057b2
--- /dev/null
+++ b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake
@@ -0,0 +1,8 @@
+FILE(REMOVE_RECURSE
+  "CMakeFiles/run_tests"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang)
+  INCLUDE(CMakeFiles/run_tests.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/build/CMakeFiles/run_tests.dir/progress.make b/build/CMakeFiles/run_tests.dir/progress.make
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/build/CMakeFiles/run_tests.dir/progress.make
@@ -0,0 +1 @@
+
diff --git a/build/CMakeFiles/tests.dir/DependInfo.cmake b/build/CMakeFiles/tests.dir/DependInfo.cmake
new file mode 100644
index 0000000000..7aff3a53c3
--- /dev/null
+++ b/build/CMakeFiles/tests.dir/DependInfo.cmake
@@ -0,0 +1,20 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+  )
+# The set of files for implicit dependencies of each language:
+
+# Preprocessor definitions for this target.
+SET(CMAKE_TARGET_DEFINITIONS
+  "ROS_BUILD_SHARED_LIBS=1"
+  )
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+  )
+
+# The include file search paths:
+SET(CMAKE_C_TARGET_INCLUDE_PATH
+  )
+SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/tests.dir/build.make b/build/CMakeFiles/tests.dir/build.make
new file mode 100644
index 0000000000..720d530001
--- /dev/null
+++ b/build/CMakeFiles/tests.dir/build.make
@@ -0,0 +1,65 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+# Utility rule file for tests.
+
+# Include the progress variables for this target.
+include CMakeFiles/tests.dir/progress.make
+
+CMakeFiles/tests:
+
+tests: CMakeFiles/tests
+tests: CMakeFiles/tests.dir/build.make
+.PHONY : tests
+
+# Rule to build all files generated by this target.
+CMakeFiles/tests.dir/build: tests
+.PHONY : CMakeFiles/tests.dir/build
+
+CMakeFiles/tests.dir/clean:
+	$(CMAKE_COMMAND) -P CMakeFiles/tests.dir/cmake_clean.cmake
+.PHONY : CMakeFiles/tests.dir/clean
+
+CMakeFiles/tests.dir/depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/tests.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : CMakeFiles/tests.dir/depend
+
diff --git a/build/CMakeFiles/tests.dir/cmake_clean.cmake b/build/CMakeFiles/tests.dir/cmake_clean.cmake
new file mode 100644
index 0000000000..a0424cfc7c
--- /dev/null
+++ b/build/CMakeFiles/tests.dir/cmake_clean.cmake
@@ -0,0 +1,8 @@
+FILE(REMOVE_RECURSE
+  "CMakeFiles/tests"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang)
+  INCLUDE(CMakeFiles/tests.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/build/CMakeFiles/tests.dir/progress.make b/build/CMakeFiles/tests.dir/progress.make
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/build/CMakeFiles/tests.dir/progress.make
@@ -0,0 +1 @@
+
diff --git a/build/CTestTestfile.cmake b/build/CTestTestfile.cmake
new file mode 100644
index 0000000000..7b55348244
--- /dev/null
+++ b/build/CTestTestfile.cmake
@@ -0,0 +1,7 @@
+# CMake generated Testfile for 
+# Source directory: /home/roman/src/Firmware/src
+# Build directory: /home/roman/src/Firmware/build
+# 
+# This file includes the relevant testing commands required for 
+# testing this directory and lists subdirectories to be tested as well.
+SUBDIRS(gtest)
diff --git a/build/Makefile b/build/Makefile
new file mode 100644
index 0000000000..c7d796cc42
--- /dev/null
+++ b/build/Makefile
@@ -0,0 +1,262 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Default target executed when no arguments are given to make.
+default_target: all
+.PHONY : default_target
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..."
+	/usr/bin/cmake -i .
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+.PHONY : edit_cache/fast
+
+# Special rule for the target install
+install: preinstall
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
+	/usr/bin/cmake -P cmake_install.cmake
+.PHONY : install
+
+# Special rule for the target install
+install/fast: preinstall/fast
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
+	/usr/bin/cmake -P cmake_install.cmake
+.PHONY : install/fast
+
+# Special rule for the target install/local
+install/local: preinstall
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..."
+	/usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake
+.PHONY : install/local
+
+# Special rule for the target install/local
+install/local/fast: install/local
+.PHONY : install/local/fast
+
+# Special rule for the target install/strip
+install/strip: preinstall
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..."
+	/usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake
+.PHONY : install/strip
+
+# Special rule for the target install/strip
+install/strip/fast: install/strip
+.PHONY : install/strip/fast
+
+# Special rule for the target list_install_components
+list_install_components:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\""
+.PHONY : list_install_components
+
+# Special rule for the target list_install_components
+list_install_components/fast: list_install_components
+.PHONY : list_install_components/fast
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+.PHONY : rebuild_cache/fast
+
+# Special rule for the target test
+test:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..."
+	/usr/bin/ctest --force-new-ctest-process $(ARGS)
+.PHONY : test
+
+# Special rule for the target test
+test/fast: test
+.PHONY : test/fast
+
+# The main all target
+all: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/CMakeFiles/progress.marks
+	$(MAKE) -f CMakeFiles/Makefile2 all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	$(MAKE) -f CMakeFiles/Makefile2 clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	$(MAKE) -f CMakeFiles/Makefile2 preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	$(MAKE) -f CMakeFiles/Makefile2 preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+#=============================================================================
+# Target rules for targets named clean_test_results
+
+# Build rule for target.
+clean_test_results: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 clean_test_results
+.PHONY : clean_test_results
+
+# fast build rule for target.
+clean_test_results/fast:
+	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build
+.PHONY : clean_test_results/fast
+
+#=============================================================================
+# Target rules for targets named doxygen
+
+# Build rule for target.
+doxygen: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 doxygen
+.PHONY : doxygen
+
+# fast build rule for target.
+doxygen/fast:
+	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build
+.PHONY : doxygen/fast
+
+#=============================================================================
+# Target rules for targets named run_tests
+
+# Build rule for target.
+run_tests: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 run_tests
+.PHONY : run_tests
+
+# fast build rule for target.
+run_tests/fast:
+	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build
+.PHONY : run_tests/fast
+
+#=============================================================================
+# Target rules for targets named tests
+
+# Build rule for target.
+tests: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 tests
+.PHONY : tests
+
+# fast build rule for target.
+tests/fast:
+	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build
+.PHONY : tests/fast
+
+#=============================================================================
+# Target rules for targets named gtest
+
+# Build rule for target.
+gtest: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 gtest
+.PHONY : gtest
+
+# fast build rule for target.
+gtest/fast:
+	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build
+.PHONY : gtest/fast
+
+#=============================================================================
+# Target rules for targets named gtest_main
+
+# Build rule for target.
+gtest_main: cmake_check_build_system
+	$(MAKE) -f CMakeFiles/Makefile2 gtest_main
+.PHONY : gtest_main
+
+# fast build rule for target.
+gtest_main/fast:
+	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build
+.PHONY : gtest_main/fast
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... clean_test_results"
+	@echo "... doxygen"
+	@echo "... edit_cache"
+	@echo "... install"
+	@echo "... install/local"
+	@echo "... install/strip"
+	@echo "... list_install_components"
+	@echo "... rebuild_cache"
+	@echo "... run_tests"
+	@echo "... test"
+	@echo "... tests"
+	@echo "... gtest"
+	@echo "... gtest_main"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+
diff --git a/build/catkin/catkin_generated/version/package.cmake b/build/catkin/catkin_generated/version/package.cmake
new file mode 100644
index 0000000000..1cbfc826b0
--- /dev/null
+++ b/build/catkin/catkin_generated/version/package.cmake
@@ -0,0 +1,9 @@
+set(_CATKIN_CURRENT_PACKAGE "catkin")
+set(catkin_VERSION "0.6.9")
+set(catkin_BUILD_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2")
+set(catkin_BUILD_DEPENDS "python-empy" "python-argparse" "python-catkin-pkg")
+set(catkin_DEPRECATED "")
+set(catkin_RUN_DEPENDS "python-argparse" "python-catkin-pkg" "gtest" "python-empy" "python-nose")
+set(catkin_MAINTAINER "Dirk Thomas <dthomas@osrfoundation.org>")
+set(catkin_BUILDTOOL_DEPENDS "cmake")
+set(catkin_RUN_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2")
\ No newline at end of file
diff --git a/build/catkin_generated/env_cached.sh b/build/catkin_generated/env_cached.sh
new file mode 100755
index 0000000000..d6be91db5c
--- /dev/null
+++ b/build/catkin_generated/env_cached.sh
@@ -0,0 +1,16 @@
+#!/usr/bin/env sh
+# generated from catkin/cmake/templates/env.sh.in
+
+if [ $# -eq 0 ] ; then
+  /bin/echo "Usage: env.sh COMMANDS"
+  /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
+  exit 1
+fi
+
+# ensure to not use different shell type which was set before
+CATKIN_SHELL=sh
+
+# source setup_cached.sh from same directory as this file
+_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
+. "$_CATKIN_SETUP_DIR/setup_cached.sh"
+exec "$@"
diff --git a/build/catkin_generated/generate_cached_setup.py b/build/catkin_generated/generate_cached_setup.py
new file mode 100644
index 0000000000..7a56cdb7f9
--- /dev/null
+++ b/build/catkin_generated/generate_cached_setup.py
@@ -0,0 +1,29 @@
+from __future__ import print_function
+import argparse
+import os
+import stat
+import sys
+
+# find the import for catkin's python package - either from source space or from an installed underlay
+if os.path.exists(os.path.join('/opt/ros/indigo/share/catkin/cmake', 'catkinConfig.cmake.in')):
+    sys.path.insert(0, os.path.join('/opt/ros/indigo/share/catkin/cmake', '..', 'python'))
+try:
+    from catkin.environment_cache import generate_environment_script
+except ImportError:
+    # search for catkin package in all workspaces and prepend to path
+    for workspace in "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';'):
+        python_path = os.path.join(workspace, 'lib/python2.7/dist-packages')
+        if os.path.isdir(os.path.join(python_path, 'catkin')):
+            sys.path.insert(0, python_path)
+            break
+    from catkin.environment_cache import generate_environment_script
+
+code = generate_environment_script('/home/roman/src/Firmware/devel/env.sh')
+
+output_filename = '/home/roman/src/Firmware/build/catkin_generated/setup_cached.sh'
+with open(output_filename, 'w') as f:
+    #print('Generate script for cached setup "%s"' % output_filename)
+    f.write('\n'.join(code))
+
+mode = os.stat(output_filename).st_mode
+os.chmod(output_filename, mode | stat.S_IXUSR)
diff --git a/build/catkin_generated/installspace/.rosinstall b/build/catkin_generated/installspace/.rosinstall
new file mode 100644
index 0000000000..a959c71efc
--- /dev/null
+++ b/build/catkin_generated/installspace/.rosinstall
@@ -0,0 +1,2 @@
+- setup-file:
+    local-name: /home/roman/src/Firmware/install/setup.sh
diff --git a/build/catkin_generated/installspace/_setup_util.py b/build/catkin_generated/installspace/_setup_util.py
new file mode 100755
index 0000000000..8db644140c
--- /dev/null
+++ b/build/catkin_generated/installspace/_setup_util.py
@@ -0,0 +1,287 @@
+#!/usr/bin/python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * 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.
+#  * Neither the name of Willow Garage, Inc. 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.
+
+'''This file generates shell code for the setup.SHELL scripts to set environment variables'''
+
+from __future__ import print_function
+import argparse
+import copy
+import errno
+import os
+import platform
+import sys
+
+CATKIN_MARKER_FILE = '.catkin'
+
+system = platform.system()
+IS_DARWIN = (system == 'Darwin')
+IS_WINDOWS = (system == 'Windows')
+
+# subfolder of workspace prepended to CMAKE_PREFIX_PATH
+ENV_VAR_SUBFOLDERS = {
+    'CMAKE_PREFIX_PATH': '',
+    'CPATH': 'include',
+    'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')],
+    'PATH': 'bin',
+    'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')],
+    'PYTHONPATH': 'lib/python2.7/dist-packages',
+}
+
+
+def rollback_env_variables(environ, env_var_subfolders):
+    '''
+    Generate shell code to reset environment variables
+    by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH.
+    This does not cover modifications performed by environment hooks.
+    '''
+    lines = []
+    unmodified_environ = copy.copy(environ)
+    for key in sorted(env_var_subfolders.keys()):
+        subfolders = env_var_subfolders[key]
+        if not isinstance(subfolders, list):
+            subfolders = [subfolders]
+        for subfolder in subfolders:
+            value = _rollback_env_variable(unmodified_environ, key, subfolder)
+            if value is not None:
+                environ[key] = value
+                lines.append(assignment(key, value))
+    if lines:
+        lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH'))
+    return lines
+
+
+def _rollback_env_variable(environ, name, subfolder):
+    '''
+    For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder.
+
+    :param subfolder: str '' or subfoldername that may start with '/'
+    :returns: the updated value of the environment variable.
+    '''
+    value = environ[name] if name in environ else ''
+    env_paths = [path for path in value.split(os.pathsep) if path]
+    value_modified = False
+    if subfolder:
+        if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)):
+            subfolder = subfolder[1:]
+        if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)):
+            subfolder = subfolder[:-1]
+    for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True):
+        path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path
+        path_to_remove = None
+        for env_path in env_paths:
+            env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path
+            if env_path_clean == path_to_find:
+                path_to_remove = env_path
+                break
+        if path_to_remove:
+            env_paths.remove(path_to_remove)
+            value_modified = True
+    new_value = os.pathsep.join(env_paths)
+    return new_value if value_modified else None
+
+
+def _get_workspaces(environ, include_fuerte=False, include_non_existing=False):
+    '''
+    Based on CMAKE_PREFIX_PATH return all catkin workspaces.
+
+    :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool``
+    '''
+    # get all cmake prefix paths
+    env_name = 'CMAKE_PREFIX_PATH'
+    value = environ[env_name] if env_name in environ else ''
+    paths = [path for path in value.split(os.pathsep) if path]
+    # remove non-workspace paths
+    workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))]
+    return workspaces
+
+
+def prepend_env_variables(environ, env_var_subfolders, workspaces):
+    '''
+    Generate shell code to prepend environment variables
+    for the all workspaces.
+    '''
+    lines = []
+    lines.append(comment('prepend folders of workspaces to environment variables'))
+
+    paths = [path for path in workspaces.split(os.pathsep) if path]
+
+    prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '')
+    lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix))
+
+    for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']):
+        subfolder = env_var_subfolders[key]
+        prefix = _prefix_env_variable(environ, key, paths, subfolder)
+        lines.append(prepend(environ, key, prefix))
+    return lines
+
+
+def _prefix_env_variable(environ, name, paths, subfolders):
+    '''
+    Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items.
+    '''
+    value = environ[name] if name in environ else ''
+    environ_paths = [path for path in value.split(os.pathsep) if path]
+    checked_paths = []
+    for path in paths:
+        if not isinstance(subfolders, list):
+            subfolders = [subfolders]
+        for subfolder in subfolders:
+            path_tmp = path
+            if subfolder:
+                path_tmp = os.path.join(path_tmp, subfolder)
+            # exclude any path already in env and any path we already added
+            if path_tmp not in environ_paths and path_tmp not in checked_paths:
+                checked_paths.append(path_tmp)
+    prefix_str = os.pathsep.join(checked_paths)
+    if prefix_str != '' and environ_paths:
+        prefix_str += os.pathsep
+    return prefix_str
+
+
+def assignment(key, value):
+    if not IS_WINDOWS:
+        return 'export %s="%s"' % (key, value)
+    else:
+        return 'set %s=%s' % (key, value)
+
+
+def comment(msg):
+    if not IS_WINDOWS:
+        return '# %s' % msg
+    else:
+        return 'REM %s' % msg
+
+
+def prepend(environ, key, prefix):
+    if key not in environ or not environ[key]:
+        return assignment(key, prefix)
+    if not IS_WINDOWS:
+        return 'export %s="%s$%s"' % (key, prefix, key)
+    else:
+        return 'set %s=%s%%%s%%' % (key, prefix, key)
+
+
+def find_env_hooks(environ, cmake_prefix_path):
+    '''
+    Generate shell code with found environment hooks
+    for the all workspaces.
+    '''
+    lines = []
+    lines.append(comment('found environment hooks in workspaces'))
+
+    generic_env_hooks = []
+    generic_env_hooks_workspace = []
+    specific_env_hooks = []
+    specific_env_hooks_workspace = []
+    generic_env_hooks_by_filename = {}
+    specific_env_hooks_by_filename = {}
+    generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh'
+    specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None
+    # remove non-workspace paths
+    workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))]
+    for workspace in reversed(workspaces):
+        env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d')
+        if os.path.isdir(env_hook_dir):
+            for filename in sorted(os.listdir(env_hook_dir)):
+                if filename.endswith('.%s' % generic_env_hook_ext):
+                    # remove previous env hook with same name if present
+                    if filename in generic_env_hooks_by_filename:
+                        i = generic_env_hooks.index(generic_env_hooks_by_filename[filename])
+                        generic_env_hooks.pop(i)
+                        generic_env_hooks_workspace.pop(i)
+                    # append env hook
+                    generic_env_hooks.append(os.path.join(env_hook_dir, filename))
+                    generic_env_hooks_workspace.append(workspace)
+                    generic_env_hooks_by_filename[filename] = generic_env_hooks[-1]
+                elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext):
+                    # remove previous env hook with same name if present
+                    if filename in specific_env_hooks_by_filename:
+                        i = specific_env_hooks.index(specific_env_hooks_by_filename[filename])
+                        specific_env_hooks.pop(i)
+                        specific_env_hooks_workspace.pop(i)
+                    # append env hook
+                    specific_env_hooks.append(os.path.join(env_hook_dir, filename))
+                    specific_env_hooks_workspace.append(workspace)
+                    specific_env_hooks_by_filename[filename] = specific_env_hooks[-1]
+    env_hooks = generic_env_hooks + specific_env_hooks
+    env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace
+    count = len(env_hooks)
+    lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count))
+    for i in range(count):
+        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i]))
+        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i]))
+    return lines
+
+
+def _parse_arguments(args=None):
+    parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.')
+    parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context')
+    return parser.parse_known_args(args=args)[0]
+
+
+if __name__ == '__main__':
+    try:
+        try:
+            args = _parse_arguments()
+        except Exception as e:
+            print(e, file=sys.stderr)
+            sys.exit(1)
+
+        # environment at generation time
+        CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';')
+        # prepend current workspace if not already part of CPP
+        base_path = os.path.dirname(__file__)
+        if base_path not in CMAKE_PREFIX_PATH:
+            CMAKE_PREFIX_PATH.insert(0, base_path)
+        CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH)
+
+        environ = dict(os.environ)
+        lines = []
+        if not args.extend:
+            lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS)
+        lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH)
+        lines += find_env_hooks(environ, CMAKE_PREFIX_PATH)
+        print('\n'.join(lines))
+
+        # need to explicitly flush the output
+        sys.stdout.flush()
+    except IOError as e:
+        # and catch potantial "broken pipe" if stdout is not writable
+        # which can happen when piping the output to a file but the disk is full
+        if e.errno == errno.EPIPE:
+            print(e, file=sys.stderr)
+            sys.exit(2)
+        raise
+
+    sys.exit(0)
diff --git a/build/catkin_generated/installspace/env.sh b/build/catkin_generated/installspace/env.sh
new file mode 100755
index 0000000000..8aa9d244ae
--- /dev/null
+++ b/build/catkin_generated/installspace/env.sh
@@ -0,0 +1,16 @@
+#!/usr/bin/env sh
+# generated from catkin/cmake/templates/env.sh.in
+
+if [ $# -eq 0 ] ; then
+  /bin/echo "Usage: env.sh COMMANDS"
+  /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
+  exit 1
+fi
+
+# ensure to not use different shell type which was set before
+CATKIN_SHELL=sh
+
+# source setup.sh from same directory as this file
+_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
+. "$_CATKIN_SETUP_DIR/setup.sh"
+exec "$@"
diff --git a/build/catkin_generated/installspace/setup.bash b/build/catkin_generated/installspace/setup.bash
new file mode 100644
index 0000000000..ff47af8f30
--- /dev/null
+++ b/build/catkin_generated/installspace/setup.bash
@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+# generated from catkin/cmake/templates/setup.bash.in
+
+CATKIN_SHELL=bash
+
+# source setup.sh from same directory as this file
+_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
+. "$_CATKIN_SETUP_DIR/setup.sh"
diff --git a/build/catkin_generated/installspace/setup.sh b/build/catkin_generated/installspace/setup.sh
new file mode 100644
index 0000000000..95e1571bef
--- /dev/null
+++ b/build/catkin_generated/installspace/setup.sh
@@ -0,0 +1,87 @@
+#!/usr/bin/env sh
+# generated from catkin/cmake/template/setup.sh.in
+
+# Sets various environment variables and sources additional environment hooks.
+# It tries it's best to undo changes from a previously sourced setup file before.
+# Supported command line options:
+# --extend: skips the undoing of changes from a previously sourced setup file
+
+# since this file is sourced either use the provided _CATKIN_SETUP_DIR
+# or fall back to the destination set at configure time
+: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/install}
+_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py"
+unset _CATKIN_SETUP_DIR
+
+if [ ! -f "$_SETUP_UTIL" ]; then
+  echo "Missing Python script: $_SETUP_UTIL"
+  return 22
+fi
+
+# detect if running on Darwin platform
+_UNAME=`uname -s`
+_IS_DARWIN=0
+if [ "$_UNAME" = "Darwin" ]; then
+  _IS_DARWIN=1
+fi
+unset _UNAME
+
+# make sure to export all environment variables
+export CMAKE_PREFIX_PATH
+export CPATH
+if [ $_IS_DARWIN -eq 0 ]; then
+  export LD_LIBRARY_PATH
+else
+  export DYLD_LIBRARY_PATH
+fi
+unset _IS_DARWIN
+export PATH
+export PKG_CONFIG_PATH
+export PYTHONPATH
+
+# remember type of shell if not already set
+if [ -z "$CATKIN_SHELL" ]; then
+  CATKIN_SHELL=sh
+fi
+
+# invoke Python script to generate necessary exports of environment variables
+_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX`
+if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then
+  echo "Could not create temporary file: $_SETUP_TMP"
+  return 1
+fi
+CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP
+_RC=$?
+if [ $_RC -ne 0 ]; then
+  if [ $_RC -eq 2 ]; then
+    echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?"
+  else
+    echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC"
+  fi
+  unset _RC
+  unset _SETUP_UTIL
+  rm -f $_SETUP_TMP
+  unset _SETUP_TMP
+  return 1
+fi
+unset _RC
+unset _SETUP_UTIL
+. $_SETUP_TMP
+rm -f $_SETUP_TMP
+unset _SETUP_TMP
+
+# source all environment hooks
+_i=0
+while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do
+  eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i
+  unset _CATKIN_ENVIRONMENT_HOOKS_$_i
+  eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
+  unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
+  # set workspace for environment hook
+  CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace
+  . "$_envfile"
+  unset CATKIN_ENV_HOOK_WORKSPACE
+  _i=$((_i + 1))
+done
+unset _i
+
+unset _CATKIN_ENVIRONMENT_HOOKS_COUNT
diff --git a/build/catkin_generated/installspace/setup.zsh b/build/catkin_generated/installspace/setup.zsh
new file mode 100644
index 0000000000..b660717663
--- /dev/null
+++ b/build/catkin_generated/installspace/setup.zsh
@@ -0,0 +1,8 @@
+#!/usr/bin/env zsh
+# generated from catkin/cmake/templates/setup.zsh.in
+
+CATKIN_SHELL=zsh
+_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
+emulate sh # emulate POSIX
+. "$_CATKIN_SETUP_DIR/setup.sh"
+emulate zsh # back to zsh mode
diff --git a/build/catkin_generated/order_packages.cmake b/build/catkin_generated/order_packages.cmake
new file mode 100644
index 0000000000..6b0e2dff92
--- /dev/null
+++ b/build/catkin_generated/order_packages.cmake
@@ -0,0 +1,10 @@
+# generated from catkin/cmake/em/order_packages.cmake.em
+
+set(CATKIN_ORDERED_PACKAGES "")
+set(CATKIN_ORDERED_PACKAGE_PATHS "")
+set(CATKIN_ORDERED_PACKAGES_IS_META "")
+set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "")
+
+set(CATKIN_MESSAGE_GENERATORS )
+
+set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "/usr/lib/pymodules/python2.7/catkin_pkg/templates/metapackage.cmake.in")
diff --git a/build/catkin_generated/order_packages.py b/build/catkin_generated/order_packages.py
new file mode 100644
index 0000000000..5cb1765802
--- /dev/null
+++ b/build/catkin_generated/order_packages.py
@@ -0,0 +1,5 @@
+# generated from catkin/cmake/template/order_packages.context.py.in
+source_root_dir = "/home/roman/src/Firmware/src"
+whitelisted_packages = "".split(';') if "" != "" else []
+blacklisted_packages = "".split(';') if "" != "" else []
+underlay_workspaces = "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';') if "/home/roman/catkin_ws/devel;/opt/ros/indigo" != "" else []
diff --git a/build/catkin_generated/setup_cached.sh b/build/catkin_generated/setup_cached.sh
new file mode 100755
index 0000000000..e03bce4ba5
--- /dev/null
+++ b/build/catkin_generated/setup_cached.sh
@@ -0,0 +1,20 @@
+#!/usr/bin/env sh
+# generated from catkin/python/catkin/environment_cache.py
+
+# based on a snapshot of the environment before and after calling the setup script
+# it emulates the modifications of the setup script without recurring computations
+
+# new environment variables
+
+# modified environment variables
+export CATKIN_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results"
+export CMAKE_PREFIX_PATH="/home/roman/src/Firmware/devel:$CMAKE_PREFIX_PATH"
+export CPATH="/home/roman/src/Firmware/devel/include:$CPATH"
+export LD_LIBRARY_PATH="/home/roman/src/Firmware/devel/lib:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib/x86_64-linux-gnu:/opt/ros/indigo/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib:/opt/ros/indigo/lib"
+export PATH="/home/roman/src/Firmware/devel/bin:$PATH"
+export PKG_CONFIG_PATH="/home/roman/src/Firmware/devel/lib/pkgconfig:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH"
+export PWD="/home/roman/src/Firmware/build"
+export PYTHONPATH="/home/roman/src/Firmware/devel/lib/python2.7/dist-packages:$PYTHONPATH"
+export ROSLISP_PACKAGE_DIRECTORIES="/home/roman/src/Firmware/devel/share/common-lisp:$ROSLISP_PACKAGE_DIRECTORIES"
+export ROS_PACKAGE_PATH="/home/roman/src/Firmware/src:$ROS_PACKAGE_PATH"
+export ROS_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results"
\ No newline at end of file
diff --git a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp
new file mode 100644
index 0000000000..26882f02aa
--- /dev/null
+++ b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp
@@ -0,0 +1,250 @@
+#!/usr/bin/env python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * 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.
+#  * Neither the name of Willow Garage, Inc. 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.
+
+from __future__ import print_function
+import os
+import sys
+
+import distutils.core
+try:
+    import setuptools
+except ImportError:
+    pass
+
+from argparse import ArgumentParser
+
+
+def _get_locations(pkgs, package_dir):
+    """
+    based on setuptools logic and the package_dir dict, builds a dict
+    of location roots for each pkg in pkgs.
+    See http://docs.python.org/distutils/setupscript.html
+
+    :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents)
+    """
+    # package_dir contains a dict {package_name: relativepath}
+    # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'}
+    #
+    # '' means where to look for any package unless a parent package
+    # is listed so package bar.pot is expected at lib2/bar/pot,
+    # whereas package sup.dee is expected at src/sup/dee
+    #
+    # if package_dir does not state anything about a package,
+    # setuptool expects the package folder to be in the root of the
+    # project
+    locations = {}
+    allprefix = package_dir.get('', '')
+    for pkg in pkgs:
+        parent_location = None
+        splits = pkg.split('.')
+        # we iterate over compound name from parent to child
+        # so once we found parent, children just append to their parent
+        for key_len in range(len(splits)):
+            key = '.'.join(splits[:key_len + 1])
+            if key not in locations:
+                if key in package_dir:
+                    locations[key] = package_dir[key]
+                elif parent_location is not None:
+                    locations[key] = parent_location
+                else:
+                    locations[key] = allprefix
+            parent_location = locations[key]
+    return locations
+
+
+def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules):
+    """
+    Generates lines to add to a cmake file which will set variables
+
+    :param version: str, format 'int.int.int'
+    :param scripts: [list of str]: relative paths to scripts
+    :param package_dir: {modulename: path}
+    :pkgs: [list of str] python_packages declared in catkin package
+    :modules: [list of str] python modules
+    """
+    prefix = '%s_SETUP_PY' % package_name
+    result = []
+    result.append(r'set(%s_VERSION "%s")' % (prefix, version))
+    result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts)))
+
+    # Remove packages with '.' separators.
+    #
+    # setuptools allows specifying submodules in other folders than
+    # their parent
+    #
+    # The symlink approach of catkin does not work with such submodules.
+    # In the common case, this does not matter as the submodule is
+    # within the containing module.  We verify this assumption, and if
+    # it passes, we remove submodule packages.
+    locations = _get_locations(pkgs, package_dir)
+    for pkgname, location in locations.items():
+        if not '.' in pkgname:
+            continue
+        splits = pkgname.split('.')
+        # hack: ignore write-combining setup.py files for msg and srv files
+        if splits[1] in ['msg', 'srv']:
+            continue
+        # check every child has the same root folder as its parent
+        parent_name = '.'.join(splits[:1])
+        if location != locations[parent_name]:
+            raise RuntimeError(
+                "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name]))
+
+    # If checks pass, remove all submodules
+    pkgs = [p for p in pkgs if '.' not in p]
+
+    resolved_pkgs = []
+    for pkg in pkgs:
+        resolved_pkgs += [os.path.join(locations[pkg], pkg)]
+
+    result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs)))
+    result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/")))
+
+    # skip modules which collide with package names
+    filtered_modules = []
+    for modname in modules:
+        splits = modname.split('.')
+        # check all parents too
+        equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))]
+        if any(equals_package):
+            continue
+        filtered_modules.append(modname)
+    module_locations = _get_locations(filtered_modules, package_dir)
+
+    result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules])))
+    result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/")))
+
+    return result
+
+
+def _create_mock_setup_function(package_name, outfile):
+    """
+    Creates a function to call instead of distutils.core.setup or
+    setuptools.setup, which just captures some args and writes them
+    into a file that can be used from cmake
+
+    :param package_name: name of the package
+    :param outfile: filename that cmake will use afterwards
+    :returns: a function to replace disutils.core.setup and setuptools.setup
+    """
+
+    def setup(*args, **kwargs):
+        '''
+        Checks kwargs and writes a scriptfile
+        '''
+        if 'version' not in kwargs:
+            sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name)
+            raise RuntimeError("version not found in setup.py")
+        version = kwargs['version']
+        package_dir = kwargs.get('package_dir', {})
+
+        pkgs = kwargs.get('packages', [])
+        scripts = kwargs.get('scripts', [])
+        modules = kwargs.get('py_modules', [])
+
+        unsupported_args = [
+            'entry_points',
+            'exclude_package_data',
+            'ext_modules ',
+            'ext_package',
+            'include_package_data',
+            'namespace_packages',
+            'setup_requires',
+            'use_2to3',
+            'zip_safe']
+        used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs]
+        if used_unsupported_args:
+            sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name))
+
+        result = generate_cmake_file(package_name=package_name,
+                                     version=version,
+                                     scripts=scripts,
+                                     package_dir=package_dir,
+                                     pkgs=pkgs,
+                                     modules=modules)
+        with open(outfile, 'w') as out:
+            out.write('\n'.join(result))
+
+    return setup
+
+
+def main():
+    """
+    Script main, parses arguments and invokes Dummy.setup indirectly.
+    """
+    parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.')
+    parser.add_argument('package_name', help='Name of catkin package')
+    parser.add_argument('setupfile_path', help='Full path to setup.py')
+    parser.add_argument('outfile', help='Where to write result to')
+
+    args = parser.parse_args()
+
+    # print("%s" % sys.argv)
+    # PACKAGE_NAME = sys.argv[1]
+    # OUTFILE = sys.argv[3]
+    # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE),
+    #      file=sys.stderr)
+
+    # print("executing %s" % args.setupfile_path)
+
+    # be sure you're in the directory containing
+    # setup.py so the sys.path manipulation works,
+    # so the import of __version__ works
+    os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path)))
+
+    # patch setup() function of distutils and setuptools for the
+    # context of evaluating setup.py
+    try:
+        fake_setup = _create_mock_setup_function(package_name=args.package_name,
+                                                outfile=args.outfile)
+
+        distutils_backup = distutils.core.setup
+        distutils.core.setup = fake_setup
+        try:
+            setuptools_backup = setuptools.setup
+            setuptools.setup = fake_setup
+        except NameError:
+            pass
+
+        with open(args.setupfile_path, 'r') as fh:
+            exec(fh.read())
+    finally:
+        distutils.core.setup = distutils_backup
+        try:
+            setuptools.setup = setuptools_backup
+        except NameError:
+            pass
+
+if __name__ == '__main__':
+    main()
diff --git a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp
new file mode 100644
index 0000000000..087d4d802e
--- /dev/null
+++ b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp
@@ -0,0 +1,56 @@
+# generated from catkin/cmake/em/order_packages.cmake.em
+@{
+import os
+try:
+    from catkin_pkg.cmake import get_metapackage_cmake_template_path
+except ImportError as e:
+    raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e)
+try:
+    from catkin_pkg.topological_order import topological_order
+except ImportError as e:
+    raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e)
+try:
+    from catkin_pkg.package import InvalidPackage
+except ImportError as e:
+    raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e)
+# vars defined in order_packages.context.py.in
+try:
+    ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces)
+except InvalidPackage as e:
+    print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"'))
+    ordered_packages = []
+fatal_error = False
+}@
+
+set(CATKIN_ORDERED_PACKAGES "")
+set(CATKIN_ORDERED_PACKAGE_PATHS "")
+set(CATKIN_ORDERED_PACKAGES_IS_META "")
+set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "")
+@[for path, package in ordered_packages]@
+@[if path is None]@
+message(FATAL_ERROR "Circular dependency in subset of packages:\n@package")
+@{
+fatal_error = True
+}@
+@[elif package.name != 'catkin']@
+list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)")
+list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))")
+list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))")
+list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')")
+@{
+deprecated = [e for e in package.exports if e.tagname == 'deprecated']
+}@
+@[if deprecated]@
+message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')")
+@[end if]@
+@[end if]@
+@[end for]@
+
+@[if not fatal_error]@
+@{
+message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]]
+}@
+set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators)))
+@[end if]@
+
+set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))")
diff --git a/build/catkin_generated/stamps/Project/package.xml.stamp b/build/catkin_generated/stamps/Project/package.xml.stamp
new file mode 100644
index 0000000000..7c2708ac3e
--- /dev/null
+++ b/build/catkin_generated/stamps/Project/package.xml.stamp
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>catkin</name>
+  <version>0.6.9</version>
+  <description>Low-level build system macros and infrastructure for ROS.</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/catkin</url>
+  <url type="bugtracker">https://github.com/ros/catkin/issues</url>
+  <url type="repository">https://github.com/ros/catkin</url>
+
+  <author>Troy Straszheim</author>
+  <author>Morten Kjaergaard</author>
+  <author>Brian Gerkey</author>
+  <author>Dirk Thomas</author>
+
+  <buildtool_depend>cmake</buildtool_depend>
+  <buildtool_export_depend>cmake</buildtool_export_depend>
+
+  <depend>python-argparse</depend>
+  <depend version_gte="0.2.2">python-catkin-pkg</depend>
+
+  <build_depend>python-empy</build_depend>
+
+  <build_export_depend>gtest</build_export_depend>
+  <build_export_depend>python-empy</build_export_depend>
+  <build_export_depend>python-nose</build_export_depend>
+
+  <test_depend>python-mock</test_depend>
+  <test_depend>python-nose</test_depend>
+
+  <export>
+    <rosdoc config="rosdoc.yaml"/>
+    <architecture_independent/>
+  </export>
+</package>
diff --git a/build/catkin_make.cache b/build/catkin_make.cache
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/build/catkin_make.cache
@@ -0,0 +1 @@
+
diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake
new file mode 100644
index 0000000000..982fd705c6
--- /dev/null
+++ b/build/cmake_install.cmake
@@ -0,0 +1,140 @@
+# Install script for directory: /home/roman/src/Firmware/src
+
+# Set the install prefix
+IF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install")
+ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  IF(BUILD_TYPE)
+    STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  ELSE(BUILD_TYPE)
+    SET(CMAKE_INSTALL_CONFIG_NAME "")
+  ENDIF(BUILD_TYPE)
+  MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+
+# Set the component getting installed.
+IF(NOT CMAKE_INSTALL_COMPONENT)
+  IF(COMPONENT)
+    MESSAGE(STATUS "Install component: \"${COMPONENT}\"")
+    SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  ELSE(COMPONENT)
+    SET(CMAKE_INSTALL_COMPONENT)
+  ENDIF(COMPONENT)
+ENDIF(NOT CMAKE_INSTALL_COMPONENT)
+
+# Install shared libraries without execute permission?
+IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  SET(CMAKE_INSTALL_SO_NO_EXE "1")
+ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  
+      if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
+        file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
+      endif()
+      if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin")
+        file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "")
+      endif()
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
+   "/home/roman/src/Firmware/install/_setup_util.py")
+  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/_setup_util.py")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
+   "/home/roman/src/Firmware/install/env.sh")
+  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/env.sh")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
+   "/home/roman/src/Firmware/install/setup.bash")
+  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.bash")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
+   "/home/roman/src/Firmware/install/setup.sh")
+  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.sh")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
+   "/home/roman/src/Firmware/install/setup.zsh")
+  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.zsh")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
+   "/home/roman/src/Firmware/install/.rosinstall")
+  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
+  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
+  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
+FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/.rosinstall")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make.bash")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+  FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make_isolated.bash")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_LOCAL_ONLY)
+  # Include the install script for each subdirectory.
+  INCLUDE("/home/roman/src/Firmware/build/gtest/cmake_install.cmake")
+
+ENDIF(NOT CMAKE_INSTALL_LOCAL_ONLY)
+
+IF(CMAKE_INSTALL_COMPONENT)
+  SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
+ELSE(CMAKE_INSTALL_COMPONENT)
+  SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
+ENDIF(CMAKE_INSTALL_COMPONENT)
+
+FILE(WRITE "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "")
+FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES})
+  FILE(APPEND "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "${file}\n")
+ENDFOREACH(file)
diff --git a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake
new file mode 100644
index 0000000000..06b7c63c13
--- /dev/null
+++ b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake
@@ -0,0 +1,16 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Relative path conversion top directories.
+SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/usr/src/gtest")
+SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build")
+
+# Force unix paths in dependencies.
+SET(CMAKE_FORCE_UNIX_PATHS 1)
+
+
+# The C and CXX include file regular expressions for this directory.
+SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
+SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
+SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
+SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake
new file mode 100644
index 0000000000..40234804c6
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake
@@ -0,0 +1,27 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+  "CXX"
+  )
+# The set of files for implicit dependencies of each language:
+SET(CMAKE_DEPENDS_CHECK_CXX
+  "/usr/src/gtest/src/gtest-all.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o"
+  )
+SET(CMAKE_CXX_COMPILER_ID "GNU")
+
+# Preprocessor definitions for this target.
+SET(CMAKE_TARGET_DEFINITIONS
+  "GTEST_CREATE_SHARED_LIBRARY=1"
+  )
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+  )
+
+# The include file search paths:
+SET(CMAKE_C_TARGET_INCLUDE_PATH
+  "/usr/src/gtest/include"
+  "/usr/src/gtest"
+  )
+SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/gtest/CMakeFiles/gtest.dir/build.make b/build/gtest/CMakeFiles/gtest.dir/build.make
new file mode 100644
index 0000000000..744feec6d6
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/build.make
@@ -0,0 +1,102 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+# Include any dependencies generated for this target.
+include gtest/CMakeFiles/gtest.dir/depend.make
+
+# Include the progress variables for this target.
+include gtest/CMakeFiles/gtest.dir/progress.make
+
+# Include the compile flags for this target's objects.
+include gtest/CMakeFiles/gtest.dir/flags.make
+
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: gtest/CMakeFiles/gtest.dir/flags.make
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: /usr/src/gtest/src/gtest-all.cc
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1)
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o"
+	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++   $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest.dir/src/gtest-all.cc.o -c /usr/src/gtest/src/gtest-all.cc
+
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest.dir/src/gtest-all.cc.i"
+	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest-all.cc > CMakeFiles/gtest.dir/src/gtest-all.cc.i
+
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest.dir/src/gtest-all.cc.s"
+	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest-all.cc -o CMakeFiles/gtest.dir/src/gtest-all.cc.s
+
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires:
+.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires
+
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires
+	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build
+.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides
+
+gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o
+
+# Object files for target gtest
+gtest_OBJECTS = \
+"CMakeFiles/gtest.dir/src/gtest-all.cc.o"
+
+# External object files for target gtest
+gtest_EXTERNAL_OBJECTS =
+
+gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o
+gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/build.make
+gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/link.txt
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest.so"
+	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest.dir/link.txt --verbose=$(VERBOSE)
+
+# Rule to build all files generated by this target.
+gtest/CMakeFiles/gtest.dir/build: gtest/libgtest.so
+.PHONY : gtest/CMakeFiles/gtest.dir/build
+
+gtest/CMakeFiles/gtest.dir/requires: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires
+.PHONY : gtest/CMakeFiles/gtest.dir/requires
+
+gtest/CMakeFiles/gtest.dir/clean:
+	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest.dir/cmake_clean.cmake
+.PHONY : gtest/CMakeFiles/gtest.dir/clean
+
+gtest/CMakeFiles/gtest.dir/depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : gtest/CMakeFiles/gtest.dir/depend
+
diff --git a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake
new file mode 100644
index 0000000000..015a1ee758
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake
@@ -0,0 +1,10 @@
+FILE(REMOVE_RECURSE
+  "CMakeFiles/gtest.dir/src/gtest-all.cc.o"
+  "libgtest.pdb"
+  "libgtest.so"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang CXX)
+  INCLUDE(CMakeFiles/gtest.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/build/gtest/CMakeFiles/gtest.dir/depend.make b/build/gtest/CMakeFiles/gtest.dir/depend.make
new file mode 100644
index 0000000000..37ac348dbd
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/depend.make
@@ -0,0 +1,2 @@
+# Empty dependencies file for gtest.
+# This may be replaced when dependencies are built.
diff --git a/build/gtest/CMakeFiles/gtest.dir/flags.make b/build/gtest/CMakeFiles/gtest.dir/flags.make
new file mode 100644
index 0000000000..fa55300acd
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/flags.make
@@ -0,0 +1,8 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# compile CXX with /usr/bin/c++
+CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest     -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra
+
+CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_EXPORTS
+
diff --git a/build/gtest/CMakeFiles/gtest.dir/link.txt b/build/gtest/CMakeFiles/gtest.dir/link.txt
new file mode 100644
index 0000000000..8745cedd2c
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/link.txt
@@ -0,0 +1 @@
+/usr/bin/c++  -fPIC    -shared -Wl,-soname,libgtest.so -o libgtest.so CMakeFiles/gtest.dir/src/gtest-all.cc.o  -L/home/roman/src/Firmware/build/gtest/src -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src 
diff --git a/build/gtest/CMakeFiles/gtest.dir/progress.make b/build/gtest/CMakeFiles/gtest.dir/progress.make
new file mode 100644
index 0000000000..781c7de277
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest.dir/progress.make
@@ -0,0 +1,2 @@
+CMAKE_PROGRESS_1 = 1
+
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake
new file mode 100644
index 0000000000..9f3beee6e1
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake
@@ -0,0 +1,28 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+  "CXX"
+  )
+# The set of files for implicit dependencies of each language:
+SET(CMAKE_DEPENDS_CHECK_CXX
+  "/usr/src/gtest/src/gtest_main.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
+  )
+SET(CMAKE_CXX_COMPILER_ID "GNU")
+
+# Preprocessor definitions for this target.
+SET(CMAKE_TARGET_DEFINITIONS
+  "GTEST_CREATE_SHARED_LIBRARY=1"
+  )
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+  "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake"
+  )
+
+# The include file search paths:
+SET(CMAKE_C_TARGET_INCLUDE_PATH
+  "/usr/src/gtest/include"
+  "/usr/src/gtest"
+  )
+SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
+SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/build.make b/build/gtest/CMakeFiles/gtest_main.dir/build.make
new file mode 100644
index 0000000000..67a7ec0180
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/build.make
@@ -0,0 +1,103 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+# Include any dependencies generated for this target.
+include gtest/CMakeFiles/gtest_main.dir/depend.make
+
+# Include the progress variables for this target.
+include gtest/CMakeFiles/gtest_main.dir/progress.make
+
+# Include the compile flags for this target's objects.
+include gtest/CMakeFiles/gtest_main.dir/flags.make
+
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: gtest/CMakeFiles/gtest_main.dir/flags.make
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: /usr/src/gtest/src/gtest_main.cc
+	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1)
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
+	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++   $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -c /usr/src/gtest/src/gtest_main.cc
+
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest_main.dir/src/gtest_main.cc.i"
+	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest_main.cc > CMakeFiles/gtest_main.dir/src/gtest_main.cc.i
+
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s: cmake_force
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest_main.dir/src/gtest_main.cc.s"
+	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest_main.cc -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.s
+
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires:
+.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires
+
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires
+	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build
+.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides
+
+gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o
+
+# Object files for target gtest_main
+gtest_main_OBJECTS = \
+"CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
+
+# External object files for target gtest_main
+gtest_main_EXTERNAL_OBJECTS =
+
+gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o
+gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/build.make
+gtest/libgtest_main.so: gtest/libgtest.so
+gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/link.txt
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest_main.so"
+	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest_main.dir/link.txt --verbose=$(VERBOSE)
+
+# Rule to build all files generated by this target.
+gtest/CMakeFiles/gtest_main.dir/build: gtest/libgtest_main.so
+.PHONY : gtest/CMakeFiles/gtest_main.dir/build
+
+gtest/CMakeFiles/gtest_main.dir/requires: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires
+.PHONY : gtest/CMakeFiles/gtest_main.dir/requires
+
+gtest/CMakeFiles/gtest_main.dir/clean:
+	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest_main.dir/cmake_clean.cmake
+.PHONY : gtest/CMakeFiles/gtest_main.dir/clean
+
+gtest/CMakeFiles/gtest_main.dir/depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : gtest/CMakeFiles/gtest_main.dir/depend
+
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake
new file mode 100644
index 0000000000..c8fe838192
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake
@@ -0,0 +1,10 @@
+FILE(REMOVE_RECURSE
+  "CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
+  "libgtest_main.pdb"
+  "libgtest_main.so"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang CXX)
+  INCLUDE(CMakeFiles/gtest_main.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/build/gtest/CMakeFiles/gtest_main.dir/depend.make
new file mode 100644
index 0000000000..1d67c1ab52
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/depend.make
@@ -0,0 +1,2 @@
+# Empty dependencies file for gtest_main.
+# This may be replaced when dependencies are built.
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/build/gtest/CMakeFiles/gtest_main.dir/flags.make
new file mode 100644
index 0000000000..93ab70f12f
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/flags.make
@@ -0,0 +1,8 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# compile CXX with /usr/bin/c++
+CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest     -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra
+
+CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_main_EXPORTS
+
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/build/gtest/CMakeFiles/gtest_main.dir/link.txt
new file mode 100644
index 0000000000..d35b7f61ac
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/link.txt
@@ -0,0 +1 @@
+/usr/bin/c++  -fPIC    -shared -Wl,-soname,libgtest_main.so -o libgtest_main.so CMakeFiles/gtest_main.dir/src/gtest_main.cc.o  -L/home/roman/src/Firmware/build/gtest/src -lpthread libgtest.so -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src:/home/roman/src/Firmware/build/gtest 
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/build/gtest/CMakeFiles/gtest_main.dir/progress.make
new file mode 100644
index 0000000000..164e1d26ca
--- /dev/null
+++ b/build/gtest/CMakeFiles/gtest_main.dir/progress.make
@@ -0,0 +1,2 @@
+CMAKE_PROGRESS_1 = 2
+
diff --git a/build/gtest/CMakeFiles/progress.marks b/build/gtest/CMakeFiles/progress.marks
new file mode 100644
index 0000000000..573541ac97
--- /dev/null
+++ b/build/gtest/CMakeFiles/progress.marks
@@ -0,0 +1 @@
+0
diff --git a/build/gtest/CTestTestfile.cmake b/build/gtest/CTestTestfile.cmake
new file mode 100644
index 0000000000..bd4b57a8bf
--- /dev/null
+++ b/build/gtest/CTestTestfile.cmake
@@ -0,0 +1,6 @@
+# CMake generated Testfile for 
+# Source directory: /usr/src/gtest
+# Build directory: /home/roman/src/Firmware/build/gtest
+# 
+# This file includes the relevant testing commands required for 
+# testing this directory and lists subdirectories to be tested as well.
diff --git a/build/gtest/Makefile b/build/gtest/Makefile
new file mode 100644
index 0000000000..bc8c23a004
--- /dev/null
+++ b/build/gtest/Makefile
@@ -0,0 +1,262 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Default target executed when no arguments are given to make.
+default_target: all
+.PHONY : default_target
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..."
+	/usr/bin/cmake -i .
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+.PHONY : edit_cache/fast
+
+# Special rule for the target install
+install: preinstall
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
+	/usr/bin/cmake -P cmake_install.cmake
+.PHONY : install
+
+# Special rule for the target install
+install/fast: preinstall/fast
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
+	/usr/bin/cmake -P cmake_install.cmake
+.PHONY : install/fast
+
+# Special rule for the target install/local
+install/local: preinstall
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..."
+	/usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake
+.PHONY : install/local
+
+# Special rule for the target install/local
+install/local/fast: install/local
+.PHONY : install/local/fast
+
+# Special rule for the target install/strip
+install/strip: preinstall
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..."
+	/usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake
+.PHONY : install/strip
+
+# Special rule for the target install/strip
+install/strip/fast: install/strip
+.PHONY : install/strip/fast
+
+# Special rule for the target list_install_components
+list_install_components:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\""
+.PHONY : list_install_components
+
+# Special rule for the target list_install_components
+list_install_components/fast: list_install_components
+.PHONY : list_install_components/fast
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+.PHONY : rebuild_cache/fast
+
+# Special rule for the target test
+test:
+	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..."
+	/usr/bin/ctest --force-new-ctest-process $(ARGS)
+.PHONY : test
+
+# Special rule for the target test
+test/fast: test
+.PHONY : test/fast
+
+# The main all target
+all: cmake_check_build_system
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/gtest/CMakeFiles/progress.marks
+	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+# Convenience name for target.
+gtest/CMakeFiles/gtest.dir/rule:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/rule
+.PHONY : gtest/CMakeFiles/gtest.dir/rule
+
+# Convenience name for target.
+gtest: gtest/CMakeFiles/gtest.dir/rule
+.PHONY : gtest
+
+# fast build rule for target.
+gtest/fast:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build
+.PHONY : gtest/fast
+
+# Convenience name for target.
+gtest/CMakeFiles/gtest_main.dir/rule:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/rule
+.PHONY : gtest/CMakeFiles/gtest_main.dir/rule
+
+# Convenience name for target.
+gtest_main: gtest/CMakeFiles/gtest_main.dir/rule
+.PHONY : gtest_main
+
+# fast build rule for target.
+gtest_main/fast:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build
+.PHONY : gtest_main/fast
+
+src/gtest-all.o: src/gtest-all.cc.o
+.PHONY : src/gtest-all.o
+
+# target to build an object file
+src/gtest-all.cc.o:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o
+.PHONY : src/gtest-all.cc.o
+
+src/gtest-all.i: src/gtest-all.cc.i
+.PHONY : src/gtest-all.i
+
+# target to preprocess a source file
+src/gtest-all.cc.i:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i
+.PHONY : src/gtest-all.cc.i
+
+src/gtest-all.s: src/gtest-all.cc.s
+.PHONY : src/gtest-all.s
+
+# target to generate assembly for a file
+src/gtest-all.cc.s:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s
+.PHONY : src/gtest-all.cc.s
+
+src/gtest_main.o: src/gtest_main.cc.o
+.PHONY : src/gtest_main.o
+
+# target to build an object file
+src/gtest_main.cc.o:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o
+.PHONY : src/gtest_main.cc.o
+
+src/gtest_main.i: src/gtest_main.cc.i
+.PHONY : src/gtest_main.i
+
+# target to preprocess a source file
+src/gtest_main.cc.i:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i
+.PHONY : src/gtest_main.cc.i
+
+src/gtest_main.s: src/gtest_main.cc.s
+.PHONY : src/gtest_main.s
+
+# target to generate assembly for a file
+src/gtest_main.cc.s:
+	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s
+.PHONY : src/gtest_main.cc.s
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... edit_cache"
+	@echo "... gtest"
+	@echo "... gtest_main"
+	@echo "... install"
+	@echo "... install/local"
+	@echo "... install/strip"
+	@echo "... list_install_components"
+	@echo "... rebuild_cache"
+	@echo "... test"
+	@echo "... src/gtest-all.o"
+	@echo "... src/gtest-all.i"
+	@echo "... src/gtest-all.s"
+	@echo "... src/gtest_main.o"
+	@echo "... src/gtest_main.i"
+	@echo "... src/gtest_main.s"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+
diff --git a/build/gtest/cmake_install.cmake b/build/gtest/cmake_install.cmake
new file mode 100644
index 0000000000..f51b656514
--- /dev/null
+++ b/build/gtest/cmake_install.cmake
@@ -0,0 +1,34 @@
+# Install script for directory: /usr/src/gtest
+
+# Set the install prefix
+IF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install")
+ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  IF(BUILD_TYPE)
+    STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  ELSE(BUILD_TYPE)
+    SET(CMAKE_INSTALL_CONFIG_NAME "")
+  ENDIF(BUILD_TYPE)
+  MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+
+# Set the component getting installed.
+IF(NOT CMAKE_INSTALL_COMPONENT)
+  IF(COMPONENT)
+    MESSAGE(STATUS "Install component: \"${COMPONENT}\"")
+    SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  ELSE(COMPONENT)
+    SET(CMAKE_INSTALL_COMPONENT)
+  ENDIF(COMPONENT)
+ENDIF(NOT CMAKE_INSTALL_COMPONENT)
+
+# Install shared libraries without execute permission?
+IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  SET(CMAKE_INSTALL_SO_NO_EXE "1")
+ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+
diff --git a/devel/.catkin b/devel/.catkin
new file mode 100644
index 0000000000..50d43da7e7
--- /dev/null
+++ b/devel/.catkin
@@ -0,0 +1 @@
+/home/roman/src/Firmware/src
\ No newline at end of file
diff --git a/devel/.rosinstall b/devel/.rosinstall
new file mode 100644
index 0000000000..fda964e6cf
--- /dev/null
+++ b/devel/.rosinstall
@@ -0,0 +1,2 @@
+- setup-file:
+    local-name: /home/roman/src/Firmware/devel/setup.sh
diff --git a/devel/_setup_util.py b/devel/_setup_util.py
new file mode 100755
index 0000000000..8db644140c
--- /dev/null
+++ b/devel/_setup_util.py
@@ -0,0 +1,287 @@
+#!/usr/bin/python
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * 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.
+#  * Neither the name of Willow Garage, Inc. 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.
+
+'''This file generates shell code for the setup.SHELL scripts to set environment variables'''
+
+from __future__ import print_function
+import argparse
+import copy
+import errno
+import os
+import platform
+import sys
+
+CATKIN_MARKER_FILE = '.catkin'
+
+system = platform.system()
+IS_DARWIN = (system == 'Darwin')
+IS_WINDOWS = (system == 'Windows')
+
+# subfolder of workspace prepended to CMAKE_PREFIX_PATH
+ENV_VAR_SUBFOLDERS = {
+    'CMAKE_PREFIX_PATH': '',
+    'CPATH': 'include',
+    'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')],
+    'PATH': 'bin',
+    'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')],
+    'PYTHONPATH': 'lib/python2.7/dist-packages',
+}
+
+
+def rollback_env_variables(environ, env_var_subfolders):
+    '''
+    Generate shell code to reset environment variables
+    by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH.
+    This does not cover modifications performed by environment hooks.
+    '''
+    lines = []
+    unmodified_environ = copy.copy(environ)
+    for key in sorted(env_var_subfolders.keys()):
+        subfolders = env_var_subfolders[key]
+        if not isinstance(subfolders, list):
+            subfolders = [subfolders]
+        for subfolder in subfolders:
+            value = _rollback_env_variable(unmodified_environ, key, subfolder)
+            if value is not None:
+                environ[key] = value
+                lines.append(assignment(key, value))
+    if lines:
+        lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH'))
+    return lines
+
+
+def _rollback_env_variable(environ, name, subfolder):
+    '''
+    For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder.
+
+    :param subfolder: str '' or subfoldername that may start with '/'
+    :returns: the updated value of the environment variable.
+    '''
+    value = environ[name] if name in environ else ''
+    env_paths = [path for path in value.split(os.pathsep) if path]
+    value_modified = False
+    if subfolder:
+        if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)):
+            subfolder = subfolder[1:]
+        if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)):
+            subfolder = subfolder[:-1]
+    for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True):
+        path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path
+        path_to_remove = None
+        for env_path in env_paths:
+            env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path
+            if env_path_clean == path_to_find:
+                path_to_remove = env_path
+                break
+        if path_to_remove:
+            env_paths.remove(path_to_remove)
+            value_modified = True
+    new_value = os.pathsep.join(env_paths)
+    return new_value if value_modified else None
+
+
+def _get_workspaces(environ, include_fuerte=False, include_non_existing=False):
+    '''
+    Based on CMAKE_PREFIX_PATH return all catkin workspaces.
+
+    :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool``
+    '''
+    # get all cmake prefix paths
+    env_name = 'CMAKE_PREFIX_PATH'
+    value = environ[env_name] if env_name in environ else ''
+    paths = [path for path in value.split(os.pathsep) if path]
+    # remove non-workspace paths
+    workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))]
+    return workspaces
+
+
+def prepend_env_variables(environ, env_var_subfolders, workspaces):
+    '''
+    Generate shell code to prepend environment variables
+    for the all workspaces.
+    '''
+    lines = []
+    lines.append(comment('prepend folders of workspaces to environment variables'))
+
+    paths = [path for path in workspaces.split(os.pathsep) if path]
+
+    prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '')
+    lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix))
+
+    for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']):
+        subfolder = env_var_subfolders[key]
+        prefix = _prefix_env_variable(environ, key, paths, subfolder)
+        lines.append(prepend(environ, key, prefix))
+    return lines
+
+
+def _prefix_env_variable(environ, name, paths, subfolders):
+    '''
+    Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items.
+    '''
+    value = environ[name] if name in environ else ''
+    environ_paths = [path for path in value.split(os.pathsep) if path]
+    checked_paths = []
+    for path in paths:
+        if not isinstance(subfolders, list):
+            subfolders = [subfolders]
+        for subfolder in subfolders:
+            path_tmp = path
+            if subfolder:
+                path_tmp = os.path.join(path_tmp, subfolder)
+            # exclude any path already in env and any path we already added
+            if path_tmp not in environ_paths and path_tmp not in checked_paths:
+                checked_paths.append(path_tmp)
+    prefix_str = os.pathsep.join(checked_paths)
+    if prefix_str != '' and environ_paths:
+        prefix_str += os.pathsep
+    return prefix_str
+
+
+def assignment(key, value):
+    if not IS_WINDOWS:
+        return 'export %s="%s"' % (key, value)
+    else:
+        return 'set %s=%s' % (key, value)
+
+
+def comment(msg):
+    if not IS_WINDOWS:
+        return '# %s' % msg
+    else:
+        return 'REM %s' % msg
+
+
+def prepend(environ, key, prefix):
+    if key not in environ or not environ[key]:
+        return assignment(key, prefix)
+    if not IS_WINDOWS:
+        return 'export %s="%s$%s"' % (key, prefix, key)
+    else:
+        return 'set %s=%s%%%s%%' % (key, prefix, key)
+
+
+def find_env_hooks(environ, cmake_prefix_path):
+    '''
+    Generate shell code with found environment hooks
+    for the all workspaces.
+    '''
+    lines = []
+    lines.append(comment('found environment hooks in workspaces'))
+
+    generic_env_hooks = []
+    generic_env_hooks_workspace = []
+    specific_env_hooks = []
+    specific_env_hooks_workspace = []
+    generic_env_hooks_by_filename = {}
+    specific_env_hooks_by_filename = {}
+    generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh'
+    specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None
+    # remove non-workspace paths
+    workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))]
+    for workspace in reversed(workspaces):
+        env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d')
+        if os.path.isdir(env_hook_dir):
+            for filename in sorted(os.listdir(env_hook_dir)):
+                if filename.endswith('.%s' % generic_env_hook_ext):
+                    # remove previous env hook with same name if present
+                    if filename in generic_env_hooks_by_filename:
+                        i = generic_env_hooks.index(generic_env_hooks_by_filename[filename])
+                        generic_env_hooks.pop(i)
+                        generic_env_hooks_workspace.pop(i)
+                    # append env hook
+                    generic_env_hooks.append(os.path.join(env_hook_dir, filename))
+                    generic_env_hooks_workspace.append(workspace)
+                    generic_env_hooks_by_filename[filename] = generic_env_hooks[-1]
+                elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext):
+                    # remove previous env hook with same name if present
+                    if filename in specific_env_hooks_by_filename:
+                        i = specific_env_hooks.index(specific_env_hooks_by_filename[filename])
+                        specific_env_hooks.pop(i)
+                        specific_env_hooks_workspace.pop(i)
+                    # append env hook
+                    specific_env_hooks.append(os.path.join(env_hook_dir, filename))
+                    specific_env_hooks_workspace.append(workspace)
+                    specific_env_hooks_by_filename[filename] = specific_env_hooks[-1]
+    env_hooks = generic_env_hooks + specific_env_hooks
+    env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace
+    count = len(env_hooks)
+    lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count))
+    for i in range(count):
+        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i]))
+        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i]))
+    return lines
+
+
+def _parse_arguments(args=None):
+    parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.')
+    parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context')
+    return parser.parse_known_args(args=args)[0]
+
+
+if __name__ == '__main__':
+    try:
+        try:
+            args = _parse_arguments()
+        except Exception as e:
+            print(e, file=sys.stderr)
+            sys.exit(1)
+
+        # environment at generation time
+        CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';')
+        # prepend current workspace if not already part of CPP
+        base_path = os.path.dirname(__file__)
+        if base_path not in CMAKE_PREFIX_PATH:
+            CMAKE_PREFIX_PATH.insert(0, base_path)
+        CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH)
+
+        environ = dict(os.environ)
+        lines = []
+        if not args.extend:
+            lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS)
+        lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH)
+        lines += find_env_hooks(environ, CMAKE_PREFIX_PATH)
+        print('\n'.join(lines))
+
+        # need to explicitly flush the output
+        sys.stdout.flush()
+    except IOError as e:
+        # and catch potantial "broken pipe" if stdout is not writable
+        # which can happen when piping the output to a file but the disk is full
+        if e.errno == errno.EPIPE:
+            print(e, file=sys.stderr)
+            sys.exit(2)
+        raise
+
+    sys.exit(0)
diff --git a/devel/env.sh b/devel/env.sh
new file mode 100755
index 0000000000..8aa9d244ae
--- /dev/null
+++ b/devel/env.sh
@@ -0,0 +1,16 @@
+#!/usr/bin/env sh
+# generated from catkin/cmake/templates/env.sh.in
+
+if [ $# -eq 0 ] ; then
+  /bin/echo "Usage: env.sh COMMANDS"
+  /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
+  exit 1
+fi
+
+# ensure to not use different shell type which was set before
+CATKIN_SHELL=sh
+
+# source setup.sh from same directory as this file
+_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
+. "$_CATKIN_SETUP_DIR/setup.sh"
+exec "$@"
diff --git a/devel/setup.bash b/devel/setup.bash
new file mode 100644
index 0000000000..ff47af8f30
--- /dev/null
+++ b/devel/setup.bash
@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+# generated from catkin/cmake/templates/setup.bash.in
+
+CATKIN_SHELL=bash
+
+# source setup.sh from same directory as this file
+_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
+. "$_CATKIN_SETUP_DIR/setup.sh"
diff --git a/devel/setup.sh b/devel/setup.sh
new file mode 100644
index 0000000000..7d80d638a6
--- /dev/null
+++ b/devel/setup.sh
@@ -0,0 +1,87 @@
+#!/usr/bin/env sh
+# generated from catkin/cmake/template/setup.sh.in
+
+# Sets various environment variables and sources additional environment hooks.
+# It tries it's best to undo changes from a previously sourced setup file before.
+# Supported command line options:
+# --extend: skips the undoing of changes from a previously sourced setup file
+
+# since this file is sourced either use the provided _CATKIN_SETUP_DIR
+# or fall back to the destination set at configure time
+: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/devel}
+_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py"
+unset _CATKIN_SETUP_DIR
+
+if [ ! -f "$_SETUP_UTIL" ]; then
+  echo "Missing Python script: $_SETUP_UTIL"
+  return 22
+fi
+
+# detect if running on Darwin platform
+_UNAME=`uname -s`
+_IS_DARWIN=0
+if [ "$_UNAME" = "Darwin" ]; then
+  _IS_DARWIN=1
+fi
+unset _UNAME
+
+# make sure to export all environment variables
+export CMAKE_PREFIX_PATH
+export CPATH
+if [ $_IS_DARWIN -eq 0 ]; then
+  export LD_LIBRARY_PATH
+else
+  export DYLD_LIBRARY_PATH
+fi
+unset _IS_DARWIN
+export PATH
+export PKG_CONFIG_PATH
+export PYTHONPATH
+
+# remember type of shell if not already set
+if [ -z "$CATKIN_SHELL" ]; then
+  CATKIN_SHELL=sh
+fi
+
+# invoke Python script to generate necessary exports of environment variables
+_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX`
+if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then
+  echo "Could not create temporary file: $_SETUP_TMP"
+  return 1
+fi
+CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP
+_RC=$?
+if [ $_RC -ne 0 ]; then
+  if [ $_RC -eq 2 ]; then
+    echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?"
+  else
+    echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC"
+  fi
+  unset _RC
+  unset _SETUP_UTIL
+  rm -f $_SETUP_TMP
+  unset _SETUP_TMP
+  return 1
+fi
+unset _RC
+unset _SETUP_UTIL
+. $_SETUP_TMP
+rm -f $_SETUP_TMP
+unset _SETUP_TMP
+
+# source all environment hooks
+_i=0
+while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do
+  eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i
+  unset _CATKIN_ENVIRONMENT_HOOKS_$_i
+  eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
+  unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
+  # set workspace for environment hook
+  CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace
+  . "$_envfile"
+  unset CATKIN_ENV_HOOK_WORKSPACE
+  _i=$((_i + 1))
+done
+unset _i
+
+unset _CATKIN_ENVIRONMENT_HOOKS_COUNT
diff --git a/devel/setup.zsh b/devel/setup.zsh
new file mode 100644
index 0000000000..b660717663
--- /dev/null
+++ b/devel/setup.zsh
@@ -0,0 +1,8 @@
+#!/usr/bin/env zsh
+# generated from catkin/cmake/templates/setup.zsh.in
+
+CATKIN_SHELL=zsh
+_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
+emulate sh # emulate POSIX
+. "$_CATKIN_SETUP_DIR/setup.sh"
+emulate zsh # back to zsh mode
diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
index 2423e47b4f..4d7487c2bc 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2
+Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50
diff --git a/uavcan b/uavcan
index c7872def16..6980ee8240 160000
--- a/uavcan
+++ b/uavcan
@@ -1 +1 @@
-Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0
+Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520

From 5ecb2262d1578d4af4e8e5da74df85ab9c9fc19d Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 28 Aug 2014 11:05:26 +0200
Subject: [PATCH 005/416] removed devel

---
 devel/.catkin        |   1 -
 devel/.rosinstall    |   2 -
 devel/_setup_util.py | 287 -------------------------------------------
 devel/env.sh         |  16 ---
 devel/setup.bash     |   8 --
 devel/setup.sh       |  87 -------------
 devel/setup.zsh      |   8 --
 7 files changed, 409 deletions(-)
 delete mode 100644 devel/.catkin
 delete mode 100644 devel/.rosinstall
 delete mode 100755 devel/_setup_util.py
 delete mode 100755 devel/env.sh
 delete mode 100644 devel/setup.bash
 delete mode 100644 devel/setup.sh
 delete mode 100644 devel/setup.zsh

diff --git a/devel/.catkin b/devel/.catkin
deleted file mode 100644
index 50d43da7e7..0000000000
--- a/devel/.catkin
+++ /dev/null
@@ -1 +0,0 @@
-/home/roman/src/Firmware/src
\ No newline at end of file
diff --git a/devel/.rosinstall b/devel/.rosinstall
deleted file mode 100644
index fda964e6cf..0000000000
--- a/devel/.rosinstall
+++ /dev/null
@@ -1,2 +0,0 @@
-- setup-file:
-    local-name: /home/roman/src/Firmware/devel/setup.sh
diff --git a/devel/_setup_util.py b/devel/_setup_util.py
deleted file mode 100755
index 8db644140c..0000000000
--- a/devel/_setup_util.py
+++ /dev/null
@@ -1,287 +0,0 @@
-#!/usr/bin/python
-
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * 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.
-#  * Neither the name of Willow Garage, Inc. 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.
-
-'''This file generates shell code for the setup.SHELL scripts to set environment variables'''
-
-from __future__ import print_function
-import argparse
-import copy
-import errno
-import os
-import platform
-import sys
-
-CATKIN_MARKER_FILE = '.catkin'
-
-system = platform.system()
-IS_DARWIN = (system == 'Darwin')
-IS_WINDOWS = (system == 'Windows')
-
-# subfolder of workspace prepended to CMAKE_PREFIX_PATH
-ENV_VAR_SUBFOLDERS = {
-    'CMAKE_PREFIX_PATH': '',
-    'CPATH': 'include',
-    'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')],
-    'PATH': 'bin',
-    'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')],
-    'PYTHONPATH': 'lib/python2.7/dist-packages',
-}
-
-
-def rollback_env_variables(environ, env_var_subfolders):
-    '''
-    Generate shell code to reset environment variables
-    by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH.
-    This does not cover modifications performed by environment hooks.
-    '''
-    lines = []
-    unmodified_environ = copy.copy(environ)
-    for key in sorted(env_var_subfolders.keys()):
-        subfolders = env_var_subfolders[key]
-        if not isinstance(subfolders, list):
-            subfolders = [subfolders]
-        for subfolder in subfolders:
-            value = _rollback_env_variable(unmodified_environ, key, subfolder)
-            if value is not None:
-                environ[key] = value
-                lines.append(assignment(key, value))
-    if lines:
-        lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH'))
-    return lines
-
-
-def _rollback_env_variable(environ, name, subfolder):
-    '''
-    For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder.
-
-    :param subfolder: str '' or subfoldername that may start with '/'
-    :returns: the updated value of the environment variable.
-    '''
-    value = environ[name] if name in environ else ''
-    env_paths = [path for path in value.split(os.pathsep) if path]
-    value_modified = False
-    if subfolder:
-        if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)):
-            subfolder = subfolder[1:]
-        if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)):
-            subfolder = subfolder[:-1]
-    for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True):
-        path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path
-        path_to_remove = None
-        for env_path in env_paths:
-            env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path
-            if env_path_clean == path_to_find:
-                path_to_remove = env_path
-                break
-        if path_to_remove:
-            env_paths.remove(path_to_remove)
-            value_modified = True
-    new_value = os.pathsep.join(env_paths)
-    return new_value if value_modified else None
-
-
-def _get_workspaces(environ, include_fuerte=False, include_non_existing=False):
-    '''
-    Based on CMAKE_PREFIX_PATH return all catkin workspaces.
-
-    :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool``
-    '''
-    # get all cmake prefix paths
-    env_name = 'CMAKE_PREFIX_PATH'
-    value = environ[env_name] if env_name in environ else ''
-    paths = [path for path in value.split(os.pathsep) if path]
-    # remove non-workspace paths
-    workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))]
-    return workspaces
-
-
-def prepend_env_variables(environ, env_var_subfolders, workspaces):
-    '''
-    Generate shell code to prepend environment variables
-    for the all workspaces.
-    '''
-    lines = []
-    lines.append(comment('prepend folders of workspaces to environment variables'))
-
-    paths = [path for path in workspaces.split(os.pathsep) if path]
-
-    prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '')
-    lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix))
-
-    for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']):
-        subfolder = env_var_subfolders[key]
-        prefix = _prefix_env_variable(environ, key, paths, subfolder)
-        lines.append(prepend(environ, key, prefix))
-    return lines
-
-
-def _prefix_env_variable(environ, name, paths, subfolders):
-    '''
-    Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items.
-    '''
-    value = environ[name] if name in environ else ''
-    environ_paths = [path for path in value.split(os.pathsep) if path]
-    checked_paths = []
-    for path in paths:
-        if not isinstance(subfolders, list):
-            subfolders = [subfolders]
-        for subfolder in subfolders:
-            path_tmp = path
-            if subfolder:
-                path_tmp = os.path.join(path_tmp, subfolder)
-            # exclude any path already in env and any path we already added
-            if path_tmp not in environ_paths and path_tmp not in checked_paths:
-                checked_paths.append(path_tmp)
-    prefix_str = os.pathsep.join(checked_paths)
-    if prefix_str != '' and environ_paths:
-        prefix_str += os.pathsep
-    return prefix_str
-
-
-def assignment(key, value):
-    if not IS_WINDOWS:
-        return 'export %s="%s"' % (key, value)
-    else:
-        return 'set %s=%s' % (key, value)
-
-
-def comment(msg):
-    if not IS_WINDOWS:
-        return '# %s' % msg
-    else:
-        return 'REM %s' % msg
-
-
-def prepend(environ, key, prefix):
-    if key not in environ or not environ[key]:
-        return assignment(key, prefix)
-    if not IS_WINDOWS:
-        return 'export %s="%s$%s"' % (key, prefix, key)
-    else:
-        return 'set %s=%s%%%s%%' % (key, prefix, key)
-
-
-def find_env_hooks(environ, cmake_prefix_path):
-    '''
-    Generate shell code with found environment hooks
-    for the all workspaces.
-    '''
-    lines = []
-    lines.append(comment('found environment hooks in workspaces'))
-
-    generic_env_hooks = []
-    generic_env_hooks_workspace = []
-    specific_env_hooks = []
-    specific_env_hooks_workspace = []
-    generic_env_hooks_by_filename = {}
-    specific_env_hooks_by_filename = {}
-    generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh'
-    specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None
-    # remove non-workspace paths
-    workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))]
-    for workspace in reversed(workspaces):
-        env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d')
-        if os.path.isdir(env_hook_dir):
-            for filename in sorted(os.listdir(env_hook_dir)):
-                if filename.endswith('.%s' % generic_env_hook_ext):
-                    # remove previous env hook with same name if present
-                    if filename in generic_env_hooks_by_filename:
-                        i = generic_env_hooks.index(generic_env_hooks_by_filename[filename])
-                        generic_env_hooks.pop(i)
-                        generic_env_hooks_workspace.pop(i)
-                    # append env hook
-                    generic_env_hooks.append(os.path.join(env_hook_dir, filename))
-                    generic_env_hooks_workspace.append(workspace)
-                    generic_env_hooks_by_filename[filename] = generic_env_hooks[-1]
-                elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext):
-                    # remove previous env hook with same name if present
-                    if filename in specific_env_hooks_by_filename:
-                        i = specific_env_hooks.index(specific_env_hooks_by_filename[filename])
-                        specific_env_hooks.pop(i)
-                        specific_env_hooks_workspace.pop(i)
-                    # append env hook
-                    specific_env_hooks.append(os.path.join(env_hook_dir, filename))
-                    specific_env_hooks_workspace.append(workspace)
-                    specific_env_hooks_by_filename[filename] = specific_env_hooks[-1]
-    env_hooks = generic_env_hooks + specific_env_hooks
-    env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace
-    count = len(env_hooks)
-    lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count))
-    for i in range(count):
-        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i]))
-        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i]))
-    return lines
-
-
-def _parse_arguments(args=None):
-    parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.')
-    parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context')
-    return parser.parse_known_args(args=args)[0]
-
-
-if __name__ == '__main__':
-    try:
-        try:
-            args = _parse_arguments()
-        except Exception as e:
-            print(e, file=sys.stderr)
-            sys.exit(1)
-
-        # environment at generation time
-        CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';')
-        # prepend current workspace if not already part of CPP
-        base_path = os.path.dirname(__file__)
-        if base_path not in CMAKE_PREFIX_PATH:
-            CMAKE_PREFIX_PATH.insert(0, base_path)
-        CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH)
-
-        environ = dict(os.environ)
-        lines = []
-        if not args.extend:
-            lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS)
-        lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH)
-        lines += find_env_hooks(environ, CMAKE_PREFIX_PATH)
-        print('\n'.join(lines))
-
-        # need to explicitly flush the output
-        sys.stdout.flush()
-    except IOError as e:
-        # and catch potantial "broken pipe" if stdout is not writable
-        # which can happen when piping the output to a file but the disk is full
-        if e.errno == errno.EPIPE:
-            print(e, file=sys.stderr)
-            sys.exit(2)
-        raise
-
-    sys.exit(0)
diff --git a/devel/env.sh b/devel/env.sh
deleted file mode 100755
index 8aa9d244ae..0000000000
--- a/devel/env.sh
+++ /dev/null
@@ -1,16 +0,0 @@
-#!/usr/bin/env sh
-# generated from catkin/cmake/templates/env.sh.in
-
-if [ $# -eq 0 ] ; then
-  /bin/echo "Usage: env.sh COMMANDS"
-  /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
-  exit 1
-fi
-
-# ensure to not use different shell type which was set before
-CATKIN_SHELL=sh
-
-# source setup.sh from same directory as this file
-_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
-. "$_CATKIN_SETUP_DIR/setup.sh"
-exec "$@"
diff --git a/devel/setup.bash b/devel/setup.bash
deleted file mode 100644
index ff47af8f30..0000000000
--- a/devel/setup.bash
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/usr/bin/env bash
-# generated from catkin/cmake/templates/setup.bash.in
-
-CATKIN_SHELL=bash
-
-# source setup.sh from same directory as this file
-_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
-. "$_CATKIN_SETUP_DIR/setup.sh"
diff --git a/devel/setup.sh b/devel/setup.sh
deleted file mode 100644
index 7d80d638a6..0000000000
--- a/devel/setup.sh
+++ /dev/null
@@ -1,87 +0,0 @@
-#!/usr/bin/env sh
-# generated from catkin/cmake/template/setup.sh.in
-
-# Sets various environment variables and sources additional environment hooks.
-# It tries it's best to undo changes from a previously sourced setup file before.
-# Supported command line options:
-# --extend: skips the undoing of changes from a previously sourced setup file
-
-# since this file is sourced either use the provided _CATKIN_SETUP_DIR
-# or fall back to the destination set at configure time
-: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/devel}
-_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py"
-unset _CATKIN_SETUP_DIR
-
-if [ ! -f "$_SETUP_UTIL" ]; then
-  echo "Missing Python script: $_SETUP_UTIL"
-  return 22
-fi
-
-# detect if running on Darwin platform
-_UNAME=`uname -s`
-_IS_DARWIN=0
-if [ "$_UNAME" = "Darwin" ]; then
-  _IS_DARWIN=1
-fi
-unset _UNAME
-
-# make sure to export all environment variables
-export CMAKE_PREFIX_PATH
-export CPATH
-if [ $_IS_DARWIN -eq 0 ]; then
-  export LD_LIBRARY_PATH
-else
-  export DYLD_LIBRARY_PATH
-fi
-unset _IS_DARWIN
-export PATH
-export PKG_CONFIG_PATH
-export PYTHONPATH
-
-# remember type of shell if not already set
-if [ -z "$CATKIN_SHELL" ]; then
-  CATKIN_SHELL=sh
-fi
-
-# invoke Python script to generate necessary exports of environment variables
-_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX`
-if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then
-  echo "Could not create temporary file: $_SETUP_TMP"
-  return 1
-fi
-CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP
-_RC=$?
-if [ $_RC -ne 0 ]; then
-  if [ $_RC -eq 2 ]; then
-    echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?"
-  else
-    echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC"
-  fi
-  unset _RC
-  unset _SETUP_UTIL
-  rm -f $_SETUP_TMP
-  unset _SETUP_TMP
-  return 1
-fi
-unset _RC
-unset _SETUP_UTIL
-. $_SETUP_TMP
-rm -f $_SETUP_TMP
-unset _SETUP_TMP
-
-# source all environment hooks
-_i=0
-while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do
-  eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i
-  unset _CATKIN_ENVIRONMENT_HOOKS_$_i
-  eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
-  unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
-  # set workspace for environment hook
-  CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace
-  . "$_envfile"
-  unset CATKIN_ENV_HOOK_WORKSPACE
-  _i=$((_i + 1))
-done
-unset _i
-
-unset _CATKIN_ENVIRONMENT_HOOKS_COUNT
diff --git a/devel/setup.zsh b/devel/setup.zsh
deleted file mode 100644
index b660717663..0000000000
--- a/devel/setup.zsh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/usr/bin/env zsh
-# generated from catkin/cmake/templates/setup.zsh.in
-
-CATKIN_SHELL=zsh
-_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
-emulate sh # emulate POSIX
-. "$_CATKIN_SETUP_DIR/setup.sh"
-emulate zsh # back to zsh mode

From 475f9a594b1088e8cb34b2d7c6f550de5a0193d7 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 23 Sep 2014 16:55:19 +0200
Subject: [PATCH 006/416] Adapted math library for use of PX4 and ROS as shared
 library. First version, it works but some things might still be ugly

---
 src/lib/mathlib/math/Limits.hpp     |  5 ++++-
 src/lib/mathlib/math/Matrix.hpp     | 20 ++++++++++++++++++++
 src/lib/mathlib/math/Quaternion.hpp |  4 ++++
 src/lib/mathlib/math/Vector.hpp     | 11 +++++++++++
 4 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd669..713cb51b56 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,7 +39,10 @@
 
 #pragma once
 
+#ifdef CONFIG_ARCH_ARM
 #include <nuttx/config.h>
+#endif
+
 #include <stdint.h>
 
 namespace math {
@@ -84,4 +87,4 @@ float __EXPORT degrees(float radians);
 
 double __EXPORT degrees(double radians);
 
-}
\ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ca931e2daf..8fb3caa0d1 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -44,7 +44,15 @@
 #define MATRIX_HPP
 
 #include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <eigen/Eigen/Dense>
+#define M_PI_2_F 1.570769
+#endif
 
 namespace math
 {
@@ -65,7 +73,11 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
+	#else
+	eigen_matrix_instance arm_mat;
+	#endif
 
 	/**
 	 * trivial ctor
@@ -352,8 +364,16 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
+		#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+		#else
+		//probably nicer if this could go into a function like "eigen_mat_mult" or so
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me  = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::VectorXf Vec  = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
+		Eigen::VectorXf Product = Me * Vec;
+		Vector<M> res(Product.data());
+		#endif
 		return res;
 	}
 };
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 21d05c7ef1..d8acc44434 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,7 +44,11 @@
 #define QUATERNION_HPP
 
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#endif
+
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 0ddf776155..51a84d27eb 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -45,7 +45,13 @@
 
 #include <stdio.h>
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <eigen/Eigen/Dense>
+#endif
 
 namespace math
 {
@@ -65,7 +71,12 @@ public:
 	/**
 	 * struct for using arm_math functions, represents column vector
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_col;
+	#else
+	eigen_matrix_instance arm_col;
+	#endif
+
 
 	/**
 	 * trivial ctor

From 0553771f4fd6fbdba43669a8f17185ed61f96a51 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 08:52:28 +0200
Subject: [PATCH 007/416] Adapted for sharded library use with ROS. Problems to
 solve: error library from PX4 does not work yet. math functions such as
 isfinite need to be shared as well. performance library needs to be shared as
 well (commented for now)

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 926a8db2ac..0019ef94d9 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,7 +45,14 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include <systemlib/err.h>
+#else
+#include<ros_error.h>
+#include <cmath>
+#define isfinite std::isfinite
+#endif
 
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
@@ -61,8 +68,8 @@ ECL_PitchController::ECL_PitchController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
+	_bodyrate_setpoint(0.0f)
+	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
 }
 
@@ -75,7 +82,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
-		perf_count(_nonfinite_input_perf);
+		//perf_count(_nonfinite_input_perf);
 		warnx("not controlling pitch");
 		return _rate_setpoint;
 	}
@@ -138,7 +145,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
 				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
 				isfinite(airspeed_max) && isfinite(scaler))) {
-		perf_count(_nonfinite_input_perf);
+		//perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 

From cfe14d78c5a9d2f80ebc0282e4bc400dcba6a795 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 08:53:23 +0200
Subject: [PATCH 008/416] Adapted for sharded library use with ROS. Problems to
 solve: error library from PX4 does not work yet. math functions such as
 isfinite need to be shared as well. performance library needs to be shared as
 well (commented for now)

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 39b9f9d03c..973e15d987 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -51,8 +51,9 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#ifdef CONFIG_ARM_ARCH
 #include <systemlib/perf_counter.h>
-
+#endif
 class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
@@ -129,7 +130,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
-	perf_counter_t _nonfinite_input_perf;
+	//perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H

From 9f9fab400efbf8ad9cd44a0ff82cb9a97706134b Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 08:57:20 +0200
Subject: [PATCH 009/416] Adapted for shared library use with ROS

---
 src/lib/ecl/ecl.h | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index aa3c5000a4..d6098dbfea 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -36,8 +36,11 @@
  * Adapter / shim layer for system calls needed by ECL
  *
  */
-
+#ifdef CONFIG_ARCH_ARM
 #include <drivers/drv_hrt.h>
-
 #define ecl_absolute_time hrt_absolute_time
 #define ecl_elapsed_time hrt_elapsed_time
+
+#else
+#include <attitude_fw/ecl_time_ROS.h>
+#endif

From 5aefe11975c0fa04ec0a921ffb54d7cf8da5c39a Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 09:01:30 +0200
Subject: [PATCH 010/416] Had to add definition of PI is used with ROS because
 cannot share math library yet-needs to be solved

---
 src/lib/mathlib/math/Limits.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index d4c892d8a0..c593936ce6 100644
--- a/src/lib/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -45,7 +45,7 @@
 
 
 namespace math {
-
+#define M_PI_F 3.14159265358979323846
 
 float __EXPORT min(float val1, float val2)
 {
@@ -143,4 +143,4 @@ double __EXPORT degrees(double radians)
 	return (radians / M_PI) * 180.0;
 }
 
-}
\ No newline at end of file
+}

From 77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 09:11:44 +0200
Subject: [PATCH 011/416] Adapted for sharded library use with ROS. Problems to
 solve: error library from PX4 does not work yet. math functions such as
 isfinite need to be shared as well. performance library needs to be shared as
 well (commented for now)

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp |  2 +-
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 15 +++++++++++----
 src/lib/ecl/attitude_fw/ecl_roll_controller.h    |  5 ++++-
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 15 +++++++++++----
 src/lib/ecl/attitude_fw/ecl_yaw_controller.h     |  5 ++++-
 5 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 0019ef94d9..b840206d58 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -75,7 +75,7 @@ ECL_PitchController::ECL_PitchController() :
 
 ECL_PitchController::~ECL_PitchController()
 {
-	perf_free(_nonfinite_input_perf);
+	//perf_free(_nonfinite_input_perf);
 }
 
 float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 94bd26f03d..1b9925f632 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,7 +45,14 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include <systemlib/err.h>
+#else
+#include<ros_error.h>
+#include <cmath>
+#define isfinite std::isfinite
+#endif
 
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
@@ -60,20 +67,20 @@ ECL_RollController::ECL_RollController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
+	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
 {
 }
 
 ECL_RollController::~ECL_RollController()
 {
-	perf_free(_nonfinite_input_perf);
+	//perf_free(_nonfinite_input_perf);
 }
 
 float ECL_RollController::control_attitude(float roll_setpoint, float roll)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(roll_setpoint) && isfinite(roll))) {
-		perf_count(_nonfinite_input_perf);
+		//perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
 
@@ -101,7 +108,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
 			isfinite(airspeed_min) && isfinite(airspeed_max) &&
 			isfinite(scaler))) {
-		perf_count(_nonfinite_input_perf);
+		//perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0799dbe03b..84e6e9fe4f 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -51,7 +51,10 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+
+#ifdef CONIG_ARCH_ARM
 #include <systemlib/perf_counter.h>
+#endif
 
 class __EXPORT ECL_RollController //XXX: create controller superclass
 {
@@ -120,7 +123,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
-	perf_counter_t _nonfinite_input_perf;
+	//perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index fe03b80651..5b023fa8f3 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,7 +44,14 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include <systemlib/err.h>
+#else
+#include<ros_error.h>
+#include <cmath>
+#define isfinite std::isfinite
+#endif
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
@@ -59,13 +66,13 @@ ECL_YawController::ECL_YawController() :
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
 	_coordinated_min_speed(1.0f),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
+	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
 {
 }
 
 ECL_YawController::~ECL_YawController()
 {
-	perf_free(_nonfinite_input_perf);
+	//perf_free(_nonfinite_input_perf);
 }
 
 float ECL_YawController::control_attitude(float roll, float pitch,
@@ -76,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
 	if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
 				isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
 				isfinite(pitch_rate_setpoint))) {
-		perf_count(_nonfinite_input_perf);
+		//perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
 //	static int counter = 0;
@@ -120,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
 				isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
 				isfinite(airspeed_max) && isfinite(scaler))) {
-		perf_count(_nonfinite_input_perf);
+		//perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 	/* get the usual dt estimate */
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index a360c14b89..61657e95be 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -50,7 +50,10 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include <systemlib/perf_counter.h>
+#endif
 
 class __EXPORT ECL_YawController //XXX: create controller superclass
 {
@@ -121,7 +124,7 @@ private:
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
 	float _coordinated_min_speed;
-	perf_counter_t _nonfinite_input_perf;
+	//perf_counter_t _nonfinite_input_perf;
 
 };
 

From 2b8a9b632555708731d93f4aa7945d19e83d3134 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 10:29:28 +0200
Subject: [PATCH 012/416] Restored performance counter functionality, ROS
 package used own source file for function definitions but per_counter.h stays
 the same

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 6 +++---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.h   | 5 ++---
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 8 ++++----
 src/lib/ecl/attitude_fw/ecl_roll_controller.h    | 6 ++----
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 8 ++++----
 src/lib/ecl/attitude_fw/ecl_yaw_controller.h     | 6 ++----
 6 files changed, 17 insertions(+), 22 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index b840206d58..8ee8b9c682 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -69,13 +69,13 @@ ECL_PitchController::ECL_PitchController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f)
-	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
 }
 
 ECL_PitchController::~ECL_PitchController()
 {
-	//perf_free(_nonfinite_input_perf);
+	perf_free(_nonfinite_input_perf);
 }
 
 float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
@@ -145,7 +145,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
 				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
 				isfinite(airspeed_max) && isfinite(scaler))) {
-		//perf_count(_nonfinite_input_perf);
+		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 973e15d987..39b9f9d03c 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -51,9 +51,8 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#ifdef CONFIG_ARM_ARCH
 #include <systemlib/perf_counter.h>
-#endif
+
 class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
@@ -130,7 +129,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
-	//perf_counter_t _nonfinite_input_perf;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 1b9925f632..6707a11ba7 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -67,20 +67,20 @@ ECL_RollController::ECL_RollController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")),
 {
 }
 
 ECL_RollController::~ECL_RollController()
 {
-	//perf_free(_nonfinite_input_perf);
+	perf_free(_nonfinite_input_perf);
 }
 
 float ECL_RollController::control_attitude(float roll_setpoint, float roll)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(roll_setpoint) && isfinite(roll))) {
-		//perf_count(_nonfinite_input_perf);
+		perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
 
@@ -108,7 +108,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
 			isfinite(airspeed_min) && isfinite(airspeed_max) &&
 			isfinite(scaler))) {
-		//perf_count(_nonfinite_input_perf);
+		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 84e6e9fe4f..dbcabd8478 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -51,10 +51,8 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-
-#ifdef CONIG_ARCH_ARM
 #include <systemlib/perf_counter.h>
-#endif
+
 
 class __EXPORT ECL_RollController //XXX: create controller superclass
 {
@@ -123,7 +121,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
-	//perf_counter_t _nonfinite_input_perf;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 5b023fa8f3..7ff8641756 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -66,13 +66,13 @@ ECL_YawController::ECL_YawController() :
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
 	_coordinated_min_speed(1.0f),
-	//_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
 {
 }
 
 ECL_YawController::~ECL_YawController()
 {
-	//perf_free(_nonfinite_input_perf);
+	perf_free(_nonfinite_input_perf);
 }
 
 float ECL_YawController::control_attitude(float roll, float pitch,
@@ -83,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch,
 	if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
 				isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
 				isfinite(pitch_rate_setpoint))) {
-		//perf_count(_nonfinite_input_perf);
+		perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
 //	static int counter = 0;
@@ -127,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
 				isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
 				isfinite(airspeed_max) && isfinite(scaler))) {
-		//perf_count(_nonfinite_input_perf);
+		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 	/* get the usual dt estimate */
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 61657e95be..c9e80930f3 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -50,10 +50,8 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-
-#ifdef CONFIG_ARCH_ARM
 #include <systemlib/perf_counter.h>
-#endif
+
 
 class __EXPORT ECL_YawController //XXX: create controller superclass
 {
@@ -124,7 +122,7 @@ private:
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
 	float _coordinated_min_speed;
-	//perf_counter_t _nonfinite_input_perf;
+	perf_counter_t _nonfinite_input_perf;
 
 };
 

From d7cf6c4319cd9522286f0a7abb73e24a3050a6f7 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 13:45:38 +0200
Subject: [PATCH 013/416] Use namespace std so that we can use the isfinite
 function, which should be available from math.h but the compiler gives an
 error that the function is undeclared

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 +++--
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 4 ++--
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 2 +-
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 8ee8b9c682..fa0f98e16f 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -51,7 +51,8 @@
 #else
 #include<ros_error.h>
 #include <cmath>
-#define isfinite std::isfinite
+//#define isfinite std::isfinite
+using namespace std;
 #endif
 
 ECL_PitchController::ECL_PitchController() :
@@ -68,7 +69,7 @@ ECL_PitchController::ECL_PitchController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
 	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
 }
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 6707a11ba7..640940c99a 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -51,7 +51,7 @@
 #else
 #include<ros_error.h>
 #include <cmath>
-#define isfinite std::isfinite
+using namespace std;
 #endif
 
 ECL_RollController::ECL_RollController() :
@@ -67,7 +67,7 @@ ECL_RollController::ECL_RollController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
 {
 }
 
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 7ff8641756..5f01cecf9a 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -50,7 +50,7 @@
 #else
 #include<ros_error.h>
 #include <cmath>
-#define isfinite std::isfinite
+using namespace std;
 #endif
 
 ECL_YawController::ECL_YawController() :

From f347e87391d35d43bdf4bdf51323ea0c53e9ead7 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 24 Sep 2014 15:54:34 +0200
Subject: [PATCH 014/416] Added base class for fixed wing attitude controller
 -> still working on it

---
 .../fw_att_control/fw_att_control_base.cpp    |  50 +++++++
 .../fw_att_control/fw_att_control_base.h      | 124 ++++++++++++++++++
 2 files changed, 174 insertions(+)
 create mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp
 create mode 100644 src/modules/fw_att_control/fw_att_control_base.h

diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
new file mode 100644
index 0000000000..4f61f02ef7
--- /dev/null
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -0,0 +1,50 @@
+/*
+ * fw_att_control_base.cpp
+ *
+ *  Created on: Sep 24, 2014
+ *      Author: roman
+ */
+
+#include "fw_att_control_base.h"
+
+
+FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
+
+	_task_should_exit(false),
+	_task_running(false),
+	_control_task(-1),
+
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
+	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+/* states */
+	_setpoint_valid(false),
+	_debug(false)
+{
+	/* safely initialize structs */
+	_att = {};
+	_accel = {};
+	_att_sp = {};
+	_manual = {};
+	_airspeed = {};
+	_vcontrol_mode = {};
+	_actuators = {};
+	_actuators_airframe = {};
+	_global_pos = {};
+	_vehicle_status = {};
+
+}
+
+
+
+FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase()
+{
+
+}
+
+void FixedwingAttitudeControlBase::control_attitude()
+{
+
+}
diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h
new file mode 100644
index 0000000000..9a2d9b1956
--- /dev/null
+++ b/src/modules/fw_att_control/fw_att_control_base.h
@@ -0,0 +1,124 @@
+/*
+ * fw_att_control_base.h
+ *
+ *  Created on: Sep 24, 2014
+ *      Author: roman
+ */
+
+#ifndef FW_ATT_CONTROL_BASE_H_
+#define FW_ATT_CONTROL_BASE_H_
+
+#include <ecl/attitude_fw/ecl_pitch_controller.h>
+#include <ecl/attitude_fw/ecl_roll_controller.h>
+#include <ecl/attitude_fw/ecl_yaw_controller.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>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <drivers/drv_accel.h>
+
+#include <systemlib/perf_counter.h>
+
+class FixedwingAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingAttitudeControlBase();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingAttitudeControlBase();
+
+
+protected:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
+	int		_control_task;			/**< task handle for sensor task */
+
+
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude 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 */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
+
+	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_debug;				/**< if set to true, print debug output */
+
+	struct {
+		float tconst;
+		float p_p;
+		float p_d;
+		float p_i;
+		float p_ff;
+		float p_rmax_pos;
+		float p_rmax_neg;
+		float p_integrator_max;
+		float p_roll_feedforward;
+		float r_p;
+		float r_d;
+		float r_i;
+		float r_ff;
+		float r_integrator_max;
+		float r_rmax;
+		float y_p;
+		float y_i;
+		float y_d;
+		float y_ff;
+		float y_roll_feedforward;
+		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */
+
+	}		_parameters;			/**< local copies of interesting parameters */
+
+
+
+
+	ECL_RollController				_roll_ctrl;
+	ECL_PitchController				_pitch_ctrl;
+	ECL_YawController				_yaw_ctrl;
+
+
+
+
+};
+
+
+
+#endif /* FW_ATT_CONTROL_BASE_H_ */

From 81a5aeb6f5cb022e6987a1651658d9b62359cccb Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 09:11:54 +0200
Subject: [PATCH 015/416] Restored to original PX4 version since the hrt_time
 functions are now also implemented in ROS

---
 src/lib/ecl/ecl.h | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index d6098dbfea..662e3a39f5 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -36,11 +36,8 @@
  * Adapter / shim layer for system calls needed by ECL
  *
  */
-#ifdef CONFIG_ARCH_ARM
+
 #include <drivers/drv_hrt.h>
 #define ecl_absolute_time hrt_absolute_time
 #define ecl_elapsed_time hrt_elapsed_time
 
-#else
-#include <attitude_fw/ecl_time_ROS.h>
-#endif

From 96b22f1ba8980ea8e1cbc405a808ae920102f064 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 10:20:15 +0200
Subject: [PATCH 016/416] Adapted uORB topic files to work with ROS (data
 stuctures are used but not the uORB functionality)

---
 src/modules/uORB/topics/actuator_controls.h         | 4 ++++
 src/modules/uORB/topics/airspeed.h                  | 4 ++++
 src/modules/uORB/topics/manual_control_setpoint.h   | 6 ++++--
 src/modules/uORB/topics/parameter_update.h          | 6 +++++-
 src/modules/uORB/topics/vehicle_attitude.h          | 4 ++++
 src/modules/uORB/topics/vehicle_attitude_setpoint.h | 4 ++++
 src/modules/uORB/topics/vehicle_control_mode.h      | 4 ++++
 src/modules/uORB/topics/vehicle_global_position.h   | 5 ++++-
 src/modules/uORB/topics/vehicle_rates_setpoint.h    | 4 ++++
 src/modules/uORB/topics/vehicle_status.h            | 4 ++++
 10 files changed, 41 insertions(+), 4 deletions(-)

diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index e768ab2f61..6c641dbcec 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -47,7 +47,9 @@
 #define TOPIC_ACTUATOR_CONTROLS_H
 
 #include <stdint.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
@@ -70,9 +72,11 @@ struct actuator_controls_s {
  */
 
 /* actuator control sets; this list can be expanded as more controllers emerge */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
+#endif
 
 #endif
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index d2ee754cdf..4c115a8113 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -40,7 +40,9 @@
 #ifndef TOPIC_AIRSPEED_H_
 #define TOPIC_AIRSPEED_H_
 
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 #include <stdint.h>
 
 /**
@@ -63,6 +65,8 @@ struct airspeed_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(airspeed);
+#endif
 
 #endif
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index dde237adc3..af5df69792 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -41,8 +41,9 @@
 #define TOPIC_MANUAL_CONTROL_SETPOINT_H_
 
 #include <stdint.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
-
+#endif
 /**
  * Switch position
  */
@@ -106,6 +107,7 @@ struct manual_control_setpoint_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(manual_control_setpoint);
-
+#endif
 #endif
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
index 68964deb0a..7afb78d49d 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/modules/uORB/topics/parameter_update.h
@@ -40,7 +40,9 @@
 #define TOPIC_PARAMETER_UPDATE_H
 
 #include <stdint.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 /**
  * @addtogroup topics
@@ -56,6 +58,8 @@ struct parameter_update_s {
  * @}
  */
 
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(parameter_update);
+#endif
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index 40328af146..7780988c84 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -44,7 +44,9 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 /**
  * @addtogroup topics
@@ -87,6 +89,8 @@ struct vehicle_attitude_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_attitude);
+#endif
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 8446e9c6e2..8b5a761433 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -42,7 +42,9 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 /**
  * @addtogroup topics
@@ -82,6 +84,8 @@ struct vehicle_attitude_setpoint_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_attitude_setpoint);
+#endif
 
 #endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 49e2ba4b56..b071e0fa3e 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -48,7 +48,9 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 #include "vehicle_status.h"
 
 /**
@@ -89,6 +91,8 @@ struct vehicle_control_mode_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_control_mode);
+#endif
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index c3bb3b8936..e8f010924d 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -45,7 +45,9 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 /**
  * @addtogroup topics
@@ -81,6 +83,7 @@ struct vehicle_global_position_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_global_position);
-
+#endif
 #endif
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
index 9f8b412a7d..cbfab89d60 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h
@@ -41,7 +41,9 @@
 #define TOPIC_VEHICLE_RATES_SETPOINT_H_
 
 #include <stdint.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 /**
  * @addtogroup topics
@@ -62,6 +64,8 @@ struct vehicle_rates_setpoint_s {
 */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_rates_setpoint);
+#endif
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b683bf98ad..973987f5c3 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -53,7 +53,9 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
+#endif
 
 /**
  * @addtogroup topics @{
@@ -234,6 +236,8 @@ struct vehicle_status_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_status);
+#endif
 
 #endif

From af290826d9957a75ff62dbd1451988228c603a45 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 10:24:04 +0200
Subject: [PATCH 017/416] Fixed so that when working with ROS the same err.h is
 included

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 +---
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 3 +--
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 3 +--
 3 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index fa0f98e16f..d1f79f0ead 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,13 +45,11 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 #ifdef CONFIG_ARCH_ARM
-#include <systemlib/err.h>
 #else
-#include<ros_error.h>
 #include <cmath>
-//#define isfinite std::isfinite
 using namespace std;
 #endif
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 640940c99a..30176f92fb 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,11 +45,10 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 #ifdef CONFIG_ARCH_ARM
-#include <systemlib/err.h>
 #else
-#include<ros_error.h>
 #include <cmath>
 using namespace std;
 #endif
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 5f01cecf9a..1b4d8486c5 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,11 +44,10 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 #ifdef CONFIG_ARCH_ARM
-#include <systemlib/err.h>
 #else
-#include<ros_error.h>
 #include <cmath>
 using namespace std;
 #endif

From a69ae3493d99250bd5a46ba1ac5f11fcf2d05069 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 10:24:39 +0200
Subject: [PATCH 018/416] Adapted so that can be used with ROS

---
 src/drivers/drv_accel.h | 64 ++++++++++++++++++++++-------------------
 1 file changed, 34 insertions(+), 30 deletions(-)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 1f98d966bd..49b104d380 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -42,12 +42,13 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
-
+#ifdef CONFIG_ARCH_ARM
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
 #define ACCEL_DEVICE_PATH	"/dev/accel"
 
+#endif
 /**
  * accel report structure.  Reads from the device must be in multiples of this
  * structure.
@@ -81,46 +82,49 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel0);
-ORB_DECLARE(sensor_accel1);
-ORB_DECLARE(sensor_accel2);
+#ifdef CONFIG_ARCH_ARM
+	ORB_DECLARE(sensor_accel0);
+	ORB_DECLARE(sensor_accel1);
+	ORB_DECLARE(sensor_accel2);
 
-/*
- * ioctl() definitions
- *
- * Accelerometer drivers also implement the generic sensor driver
- * interfaces from drv_sensor.h
- */
+	/*
+	 * ioctl() definitions
+	 *
+	 * Accelerometer drivers also implement the generic sensor driver
+	 * interfaces from drv_sensor.h
+	 */
 
-#define _ACCELIOCBASE		(0x2100)
-#define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
+	#define _ACCELIOCBASE		(0x2100)
+	#define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
 
 
-/** set the accel internal sample rate to at least (arg) Hz */
-#define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
+	/** set the accel internal sample rate to at least (arg) Hz */
+	#define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
 
-/** return the accel internal sample rate in Hz */
-#define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
+	/** return the accel internal sample rate in Hz */
+	#define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
 
-/** set the accel internal lowpass filter to no lower than (arg) Hz */
-#define ACCELIOCSLOWPASS	_ACCELIOC(2)
+	/** set the accel internal lowpass filter to no lower than (arg) Hz */
+	#define ACCELIOCSLOWPASS	_ACCELIOC(2)
 
-/** return the accel internal lowpass filter in Hz */
-#define ACCELIOCGLOWPASS	_ACCELIOC(3)
+	/** return the accel internal lowpass filter in Hz */
+	#define ACCELIOCGLOWPASS	_ACCELIOC(3)
 
-/** set the accel scaling constants to the structure pointed to by (arg) */
-#define ACCELIOCSSCALE		_ACCELIOC(5)
+	/** set the accel scaling constants to the structure pointed to by (arg) */
+	#define ACCELIOCSSCALE		_ACCELIOC(5)
 
-/** get the accel scaling constants into the structure pointed to by (arg) */
-#define ACCELIOCGSCALE		_ACCELIOC(6)
+	/** get the accel scaling constants into the structure pointed to by (arg) */
+	#define ACCELIOCGSCALE		_ACCELIOC(6)
 
-/** set the accel measurement range to handle at least (arg) g */
-#define ACCELIOCSRANGE		_ACCELIOC(7)
+	/** set the accel measurement range to handle at least (arg) g */
+	#define ACCELIOCSRANGE		_ACCELIOC(7)
 
-/** get the current accel measurement range in g */
-#define ACCELIOCGRANGE		_ACCELIOC(8)
+	/** get the current accel measurement range in g */
+	#define ACCELIOCGRANGE		_ACCELIOC(8)
 
-/** get the result of a sensor self-test */
-#define ACCELIOCSELFTEST	_ACCELIOC(9)
+	/** get the result of a sensor self-test */
+	#define ACCELIOCSELFTEST	_ACCELIOC(9)
+
+#endif
 
 #endif /* _DRV_ACCEL_H */

From 6329ca1a70b220b99ea793f416aedc8ab6fbccff Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 10:25:57 +0200
Subject: [PATCH 019/416] Adapted so that this header can also be used in a ROS
 environment

---
 src/modules/systemlib/err.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index ca13d62655..2a201ee80b 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -67,6 +67,7 @@
 
 #include <stdarg.h>
 
+#ifdef CONFIG_ARCH_ARM
 __BEGIN_DECLS
 
 __EXPORT const char *getprogname(void);
@@ -86,4 +87,8 @@ __EXPORT void	vwarnx(const char *fmt, va_list)		__attribute__((format(printf, 1,
 
 __END_DECLS
 
+#else	//we are using ROS (should make a variable!!!)
+#include <ros/ros.h>
+#define warnx ROS_WARN
+#endif
 #endif

From 43d9ebc231501fe2d3fe6541f1079d2d019a2698 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 10:28:49 +0200
Subject: [PATCH 020/416] Added control_attitude function and cleaned up

---
 .../fw_att_control/fw_att_control_base.cpp    | 225 ++++++++++++++++--
 1 file changed, 207 insertions(+), 18 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
index 4f61f02ef7..7218e4e9bb 100644
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -6,23 +6,23 @@
  */
 
 #include "fw_att_control_base.h"
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/err.h>
 
+using namespace std;
 
 FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
 
-	_task_should_exit(false),
-	_task_running(false),
-	_control_task(-1),
+		_task_should_exit(false), _task_running(false), _control_task(-1),
 
-
-/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
-	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
-/* states */
-	_setpoint_valid(false),
-	_debug(false)
-{
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+		/* states */
+		_setpoint_valid(false), _debug(false) {
 	/* safely initialize structs */
 	_att = {};
 	_accel = {};
@@ -37,14 +37,203 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
 
 }
 
-
-
-FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase()
-{
+FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() {
 
 }
 
-void FixedwingAttitudeControlBase::control_attitude()
-{
+void FixedwingAttitudeControlBase::control_attitude() {
+	bool lock_integrator = false;
+	static int loop_counter = 0;
+	/* scale around tuning airspeed */
+
+	float airspeed;
+
+	/* if airspeed is not updating, we assume the normal average speed */
+	if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s)
+			|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+		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);
+	}
+
+	/*
+	 * For scaling our actuators using anything less than the min (close to stall)
+	 * speed doesn't make any sense - its the strongest reasonable deflection we
+	 * want to do in flight and its the baseline a human pilot would choose.
+	 *
+	 * Forcing the scaling to this value allows reasonable handheld tests.
+	 */
+
+	float airspeed_scaling = _parameters.airspeed_trim
+			/ ((airspeed < _parameters.airspeed_min) ?
+					_parameters.airspeed_min : airspeed);
+
+	float roll_sp = _parameters.rollsp_offset_rad;
+	float pitch_sp = _parameters.pitchsp_offset_rad;
+	float throttle_sp = 0.0f;
+
+	if (_vcontrol_mode.flag_control_velocity_enabled
+			|| _vcontrol_mode.flag_control_position_enabled) {
+		/* read in attitude setpoint from attitude setpoint uorb topic */
+		roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+		pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+		throttle_sp = _att_sp.thrust;
+
+		/* reset integrals where needed */
+		if (_att_sp.roll_reset_integral) {
+			_roll_ctrl.reset_integrator();
+		}
+		if (_att_sp.pitch_reset_integral) {
+			_pitch_ctrl.reset_integrator();
+		}
+		if (_att_sp.yaw_reset_integral) {
+			_yaw_ctrl.reset_integrator();
+		}
+	} else {
+		/*
+		 * Scale down roll and pitch as the setpoints are radians
+		 * and a typical remote can only do around 45 degrees, the mapping is
+		 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
+		 *
+		 * With this mapping the stick angle is a 1:1 representation of
+		 * the commanded attitude.
+		 *
+		 * The trim gets subtracted here from the manual setpoint to get
+		 * the intended attitude setpoint. Later, after the rate control step the
+		 * trim is added again to the control signal.
+		 */
+		roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+				+ _parameters.rollsp_offset_rad;
+		pitch_sp = -(_manual.x * _parameters.man_pitch_max
+				- _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
+		throttle_sp = _manual.z;
+		_actuators.control[4] = _manual.flaps;
+
+		/*
+		 * in manual mode no external source should / does emit attitude setpoints.
+		 * emit the manual setpoint here to allow attitude controller tuning
+		 * in attitude control mode.
+		 */
+		struct vehicle_attitude_setpoint_s att_sp;
+		att_sp.timestamp = hrt_absolute_time();
+		att_sp.roll_body = roll_sp;
+		att_sp.pitch_body = pitch_sp;
+		att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
+		att_sp.thrust = throttle_sp;
+
+	}
+
+	/* If the aircraft is on ground reset the integrators */
+	if (_vehicle_status.condition_landed) {
+		_roll_ctrl.reset_integrator();
+		_pitch_ctrl.reset_integrator();
+		_yaw_ctrl.reset_integrator();
+	}
+
+	/* 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 = _att.R[0][0] * _global_pos.vel_n
+				+ _att.R[1][0] * _global_pos.vel_e
+				+ _att.R[2][0] * _global_pos.vel_d;
+		speed_body_v = _att.R[0][1] * _global_pos.vel_n
+				+ _att.R[1][1] * _global_pos.vel_e
+				+ _att.R[2][1] * _global_pos.vel_d;
+		speed_body_w = _att.R[0][2] * _global_pos.vel_n
+				+ _att.R[1][2] * _global_pos.vel_e
+				+ _att.R[2][2] * _global_pos.vel_d;
+	} else {
+		if (_debug && loop_counter % 10 == 0) {
+			warnx("Did not get a valid R\n");
+		}
+	}
+
+	/* Run attitude controllers */
+	if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+		_roll_ctrl.control_attitude(roll_sp, _att.roll);
+		_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+		_yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u,
+				speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(),
+				_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+		/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
+		float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed,
+				_att.yawspeed, _yaw_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[0] =
+				(isfinite(roll_u)) ?
+						roll_u + _parameters.trim_roll : _parameters.trim_roll;
+		if (!isfinite(roll_u)) {
+			_roll_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("roll_u %.4f", (double) roll_u);
+			}
+		}
+
+		float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+				_att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[1] =
+				(isfinite(pitch_u)) ?
+						pitch_u + _parameters.trim_pitch :
+						_parameters.trim_pitch;
+		if (!isfinite(pitch_u)) {
+			_pitch_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+						" airspeed %.4f, airspeed_scaling %.4f,"
+						" roll_sp %.4f, pitch_sp %.4f,"
+						" _roll_ctrl.get_desired_rate() %.4f,"
+						" _pitch_ctrl.get_desired_rate() %.4f"
+						" att_sp.roll_body %.4f", (double) pitch_u,
+						(double) _yaw_ctrl.get_desired_rate(),
+						(double) airspeed, (double) airspeed_scaling,
+						(double) roll_sp, (double) pitch_sp,
+						(double) _roll_ctrl.get_desired_rate(),
+						(double) _pitch_ctrl.get_desired_rate(),
+						(double) _att_sp.roll_body);
+			}
+		}
+
+		float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+				_att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[2] =
+				(isfinite(yaw_u)) ?
+						yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+		if (!isfinite(yaw_u)) {
+			_yaw_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("yaw_u %.4f", (double) yaw_u);
+			}
+		}
+
+		/* throttle passed through */
+		_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+		if (!isfinite(throttle_sp)) {
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("throttle_sp %.4f", (double) throttle_sp);
+			}
+		}
+	} else {
+		perf_count(_nonfinite_input_perf);
+		if (_debug && loop_counter % 10 == 0) {
+			warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
+					(double) roll_sp, (double) pitch_sp);
+		}
+	}
 
 }

From 43056258839c67590dec811e923553fad66a0f4b Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 25 Sep 2014 10:29:30 +0200
Subject: [PATCH 021/416] Added control_attitude function

---
 src/modules/fw_att_control/fw_att_control_base.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h
index 9a2d9b1956..6e071fe201 100644
--- a/src/modules/fw_att_control/fw_att_control_base.h
+++ b/src/modules/fw_att_control/fw_att_control_base.h
@@ -114,6 +114,8 @@ protected:
 	ECL_PitchController				_pitch_ctrl;
 	ECL_YawController				_yaw_ctrl;
 
+	void control_attitude();
+
 
 
 

From 020a8292155cd2821cb66a9709a8c7c18524bdac Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Sat, 27 Sep 2014 12:00:15 +0200
Subject: [PATCH 022/416] Adapted for shared library use with ROS

---
 src/lib/mathlib/math/Matrix.hpp | 114 +++++++++++++++++++-------------
 1 file changed, 68 insertions(+), 46 deletions(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 8fb3caa0d1..34fc4604ac 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -54,10 +54,9 @@
 #define M_PI_2_F 1.570769
 #endif
 
-namespace math
-{
+namespace math {
 
-template <unsigned int M, unsigned int N>
+template<unsigned int M, unsigned int N>
 class __EXPORT Matrix;
 
 // MxN matrix with float elements
@@ -73,19 +72,19 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
-	#ifdef CONFIG_ARCH_ARM
+#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
-	#else
+#else
 	eigen_matrix_instance arm_mat;
-	#endif
+#endif
 
 	/**
 	 * trivial ctor
 	 * Initializes the elements to zero.
 	 */
-	MatrixBase() : 
-		data{},
-		arm_mat{M, N, &data[0][0]}
+	MatrixBase() :
+	data {},
+	arm_mat {M, N, &data[0][0]}
 	{
 	}
 
@@ -95,19 +94,19 @@ public:
 	 * copyt ctor
 	 */
 	MatrixBase(const MatrixBase<M, N> &m) :
-		arm_mat{M, N, &data[0][0]}
+	arm_mat {M, N, &data[0][0]}
 	{
 		memcpy(data, m.data, sizeof(data));
 	}
 
 	MatrixBase(const float *d) :
-		arm_mat{M, N, &data[0][0]}
+	arm_mat {M, N, &data[0][0]}
 	{
 		memcpy(data, d, sizeof(data));
 	}
 
-	MatrixBase(const float d[M][N]) : 
-		arm_mat{M, N, &data[0][0]}
+	MatrixBase(const float d[M][N]) :
+	arm_mat {M, N, &data[0][0]}
 	{
 		memcpy(data, d, sizeof(data));
 	}
@@ -159,9 +158,9 @@ public:
 	 */
 	bool operator ==(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
-					return false;
+		for (unsigned int j = 0; j < N; j++)
+		if (data[i][j] != m.data[i][j])
+		return false;
 
 		return true;
 	}
@@ -171,9 +170,9 @@ public:
 	 */
 	bool operator !=(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
-					return true;
+		for (unsigned int j = 0; j < N; j++)
+		if (data[i][j] != m.data[i][j])
+		return true;
 
 		return false;
 	}
@@ -193,8 +192,8 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				res.data[i][j] = -data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		res.data[i][j] = -data[i][j];
 
 		return res;
 	}
@@ -206,16 +205,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				res.data[i][j] = data[i][j] + m.data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		res.data[i][j] = data[i][j] + m.data[i][j];
 
 		return res;
 	}
 
 	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				data[i][j] += m.data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		data[i][j] += m.data[i][j];
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -227,16 +226,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				res.data[i][j] = data[i][j] - m.data[i][j];
+		for (unsigned int j = 0; j < N; j++)
+		res.data[i][j] = data[i][j] - m.data[i][j];
 
 		return res;
 	}
 
 	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				data[i][j] -= m.data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		data[i][j] -= m.data[i][j];
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -248,16 +247,17 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				res.data[i][j] = data[i][j] * num;
+		for (unsigned int j = 0; j < N; j++)
+		res.data[i][j] = data[i][j] * num;
 
 		return res;
+
 	}
 
 	Matrix<M, N> &operator *=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				data[i][j] *= num;
+		for (unsigned int j = 0; j < N; j++)
+		data[i][j] *= num;
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -266,16 +266,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				res[i][j] = data[i][j] / num;
+		for (unsigned int j = 0; j < N; j++)
+		res[i][j] = data[i][j] / num;
 
 		return res;
 	}
 
 	Matrix<M, N> &operator /=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				data[i][j] /= num;
+		for (unsigned int j = 0; j < N; j++)
+		data[i][j] /= num;
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -285,27 +285,49 @@ public:
 	 */
 	template <unsigned int P>
 	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, P> res;
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::Matrix<float,N,P,Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float,N,P,Eigen::RowMajor> >(m.arm_mat.pData);
+		Eigen::Matrix<float,M,P,Eigen::RowMajor> Product = Me * Him;
+		Matrix<M, P> res(Product.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<N, M> res;
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float,N,M,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,N,M,Eigen::RowMajor> >(this->arm_mat.pData);
+		Me.transposeInPlace();
+		Matrix<N, M> res(Me.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * invert the matrix
 	 */
 	Matrix<M, N> inversed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, N> res;
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea
+		Matrix<M, N> res(MyInverse.data());
+		return res;
+#endif
 	}
 
 	/**
@@ -323,7 +345,7 @@ public:
 		unsigned int n = (M < N) ? M : N;
 
 		for (unsigned int i = 0; i < n; i++)
-			data[i][i] = 1;
+		data[i][i] = 1;
 	}
 
 	void print(void) {
@@ -331,7 +353,7 @@ public:
 			printf("[ ");
 
 			for (unsigned int j = 0; j < N; j++)
-				printf("%.3f\t", data[i][j]);
+			printf("%.3f\t", data[i][j]);
 
 			printf(" ]\n");
 		}
@@ -364,16 +386,16 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
-		#ifdef CONFIG_ARCH_ARM
+#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
-		#else
+#else
 		//probably nicer if this could go into a function like "eigen_mat_mult" or so
-		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me  = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
-		Eigen::VectorXf Vec  = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
 		Eigen::VectorXf Product = Me * Vec;
 		Vector<M> res(Product.data());
-		#endif
+#endif
 		return res;
 	}
 };
@@ -405,8 +427,8 @@ public:
 	 */
 	Vector<3> operator *(const Vector<3> &v) const {
 		Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
-			      data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
-			      data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
+				data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
+				data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
 		return res;
 	}
 

From c8b1c5b119e6552e8e5420ca8fecd24b2a3ad4a2 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Sat, 27 Sep 2014 12:01:11 +0200
Subject: [PATCH 023/416] Adapted for shared library use with ROS

---
 src/lib/geo/geo.h                             |   6 +-
 src/lib/geo_lookup/geo_mag_declination.h      |   4 +
 .../fw_att_control/fw_att_control_main.cpp    | 403 +++++++++---------
 src/modules/uORB/topics/actuator_armed.h      |   6 +-
 4 files changed, 219 insertions(+), 200 deletions(-)

diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 2311e0a7c9..ff2d92389f 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -45,14 +45,16 @@
 
 #pragma once
 
+#ifdef CONFIG_ARCH_ARM
 #include "uORB/topics/fence.h"
 #include "uORB/topics/vehicle_global_position.h"
 
 __BEGIN_DECLS
-
+#endif
 #include "geo_lookup/geo_mag_declination.h"
 
 #include <stdbool.h>
+#include <stdint.h>
 
 #define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
 #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
@@ -276,4 +278,6 @@ __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
 
+#ifdef CONFIG_ARCH_ARM
 __END_DECLS
+#endif
diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h
index 0ac062d6d4..d79b784121 100644
--- a/src/lib/geo_lookup/geo_mag_declination.h
+++ b/src/lib/geo_lookup/geo_mag_declination.h
@@ -40,8 +40,12 @@
 
 #pragma once
 
+#ifdef CONFIG_ARCH_ARM
 __BEGIN_DECLS
 
 __EXPORT float get_mag_declination(float lat, float lon);
 
 __END_DECLS
+#else
+float get_mag_declination(float lat, float lon);
+#endif
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index c60d8d3489..5cff390e2c 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -83,8 +83,7 @@
  */
 extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
 
-class FixedwingAttitudeControl
-{
+class FixedwingAttitudeControl {
 public:
 	/**
 	 * Constructor
@@ -101,54 +100,56 @@ public:
 	 *
 	 * @return	OK on success.
 	 */
-	int		start();
+	int start();
 
 	/**
 	 * Task status
 	 *
 	 * @return	true if the mainloop is running
 	 */
-	bool		task_running() { return _task_running; }
+	bool task_running() {
+		return _task_running;
+	}
 
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_control_task;			/**< task handle for sensor task */
+	bool _task_should_exit; /**< if true, sensor task should exit */
+	bool _task_running; /**< if true, task is running in its mainloop */
+	int _control_task; /**< task handle for sensor task */
 
-	int		_att_sub;			/**< vehicle attitude 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 */
-	int		_global_pos_sub;		/**< global position subscription */
-	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int _att_sub; /**< vehicle attitude 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 */
+	int _global_pos_sub; /**< global position subscription */
+	int _vehicle_status_sub; /**< vehicle status subscription */
 
-	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
-	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
-	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
-	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
+	orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
+	orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
+	orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
+	orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude 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 */
-	struct vehicle_global_position_s		_global_pos;		/**< global position */
-	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+	struct vehicle_attitude_s _att; /**< vehicle attitude */
+	struct accel_report _accel; /**< body frame accelerations */
+	struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude 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 */
+	struct vehicle_global_position_s _global_pos; /**< global position */
+	struct vehicle_status_s _vehicle_status; /**< vehicle status */
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
-	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
+	perf_counter_t _loop_perf; /**< loop performance counter */
+	perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
+	perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
 
-	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-	bool		_debug;				/**< if set to true, print debug output */
+	bool _setpoint_valid; /**< flag if the position control setpoint is valid */
+	bool _debug; /**< if set to true, print debug output */
 
 	struct {
 		float tconst;
@@ -182,14 +183,14 @@ private:
 		float trim_roll;
 		float trim_pitch;
 		float trim_yaw;
-		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
-		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
-		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
-		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
-		float man_roll_max;						/**< Max Roll in rad */
-		float man_pitch_max;					/**< Max Pitch in rad */
+		float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
+		float man_roll_max; /**< Max Roll in rad */
+		float man_pitch_max; /**< Max Pitch in rad */
 
-	}		_parameters;			/**< local copies of interesting parameters */
+	} _parameters; /**< local copies of interesting parameters */
 
 	struct {
 
@@ -228,75 +229,71 @@ private:
 		param_t pitchsp_offset_deg;
 		param_t man_roll_max;
 		param_t man_pitch_max;
-	}		_parameter_handles;		/**< handles for interesting parameters */
-
-
-	ECL_RollController				_roll_ctrl;
-	ECL_PitchController				_pitch_ctrl;
-	ECL_YawController				_yaw_ctrl;
+	} _parameter_handles; /**< handles for interesting parameters */
 
+	ECL_RollController _roll_ctrl;
+	ECL_PitchController _pitch_ctrl;
+	ECL_YawController _yaw_ctrl;
 
 	/**
 	 * Update our local parameter cache.
 	 */
-	int		parameters_update();
+	int parameters_update();
 
 	/**
 	 * Update control outputs
 	 *
 	 */
-	void		control_update();
+	void control_update();
 
 	/**
 	 * Check for changes in vehicle control mode.
 	 */
-	void		vehicle_control_mode_poll();
+	void vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in manual inputs.
 	 */
-	void		vehicle_manual_poll();
-
+	void vehicle_manual_poll();
 
 	/**
 	 * Check for airspeed updates.
 	 */
-	void		vehicle_airspeed_poll();
+	void vehicle_airspeed_poll();
 
 	/**
 	 * Check for accel updates.
 	 */
-	void		vehicle_accel_poll();
+	void vehicle_accel_poll();
 
 	/**
 	 * Check for set triplet updates.
 	 */
-	void		vehicle_setpoint_poll();
+	void vehicle_setpoint_poll();
 
 	/**
 	 * Check for global position updates.
 	 */
-	void		global_pos_poll();
+	void global_pos_poll();
 
 	/**
 	 * Check for vehicle status updates.
 	 */
-	void		vehicle_status_poll();
+	void vehicle_status_poll();
 
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
+	static void task_main_trampoline(int argc, char *argv[]);
 
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main();
+	void task_main();
 
 };
 
-namespace att_control
-{
+namespace att_control {
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -304,39 +301,28 @@ namespace att_control
 #endif
 static const int ERROR = -1;
 
-FixedwingAttitudeControl	*g_control = nullptr;
+FixedwingAttitudeControl *g_control = nullptr;
 }
 
 FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
-	_task_should_exit(false),
-	_task_running(false),
-	_control_task(-1),
+		_task_should_exit(false), _task_running(false), _control_task(-1),
 
-/* subscriptions */
-	_att_sub(-1),
-	_accel_sub(-1),
-	_airspeed_sub(-1),
-	_vcontrol_mode_sub(-1),
-	_params_sub(-1),
-	_manual_sub(-1),
-	_global_pos_sub(-1),
-	_vehicle_status_sub(-1),
+		/* subscriptions */
+		_att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(
+				-1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub(
+				-1),
 
-/* publications */
-	_rate_sp_pub(-1),
-	_attitude_sp_pub(-1),
-	_actuators_0_pub(-1),
-	_actuators_1_pub(-1),
+		/* publications */
+		_rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub(
+				-1),
 
-/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
-	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
-/* states */
-	_setpoint_valid(false),
-	_debug(false)
-{
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+		/* states */
+		_setpoint_valid(false), _debug(false) {
 	/* safely initialize structs */
 	_att = {};
 	_accel = {};
@@ -349,7 +335,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_global_pos = {};
 	_vehicle_status = {};
 
-
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
 	_parameter_handles.p_p = param_find("FW_PR_P");
 	_parameter_handles.p_i = param_find("FW_PR_I");
@@ -390,8 +375,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	parameters_update();
 }
 
-FixedwingAttitudeControl::~FixedwingAttitudeControl()
-{
+FixedwingAttitudeControl::~FixedwingAttitudeControl() {
 	if (_control_task != -1) {
 
 		/* task wakes up every 100ms or so at the longest */
@@ -419,9 +403,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
 	att_control::g_control = nullptr;
 }
 
-int
-FixedwingAttitudeControl::parameters_update()
-{
+int FixedwingAttitudeControl::parameters_update() {
 
 	param_get(_parameter_handles.tconst, &(_parameters.tconst));
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
@@ -429,21 +411,26 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
-	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
-	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
+	param_get(_parameter_handles.p_integrator_max,
+			&(_parameters.p_integrator_max));
+	param_get(_parameter_handles.p_roll_feedforward,
+			&(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
 	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
 
-	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
+	param_get(_parameter_handles.r_integrator_max,
+			&(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
 	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
-	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
-	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_integrator_max,
+			&(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed,
+			&(_parameters.y_coordinated_min_speed));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
@@ -453,16 +440,19 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
 	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
 	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
-	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
-	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
-	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
-	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.rollsp_offset_deg,
+			&(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg,
+			&(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(
+			_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(
+			_parameters.pitchsp_offset_deg);
 	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
 	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
-
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -492,9 +482,7 @@ FixedwingAttitudeControl::parameters_update()
 	return OK;
 }
 
-void
-FixedwingAttitudeControl::vehicle_control_mode_poll()
-{
+void FixedwingAttitudeControl::vehicle_control_mode_poll() {
 	bool vcontrol_mode_updated;
 
 	/* Check HIL state if vehicle status has changed */
@@ -502,13 +490,12 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 
 	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub,
+				&_vcontrol_mode);
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_manual_poll()
-{
+void FixedwingAttitudeControl::vehicle_manual_poll() {
 	bool manual_updated;
 
 	/* get pilots inputs */
@@ -520,9 +507,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_airspeed_poll()
-{
+void FixedwingAttitudeControl::vehicle_airspeed_poll() {
 	/* check if there is a new position */
 	bool airspeed_updated;
 	orb_check(_airspeed_sub, &airspeed_updated);
@@ -533,9 +518,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_accel_poll()
-{
+void FixedwingAttitudeControl::vehicle_accel_poll() {
 	/* check if there is a new position */
 	bool accel_updated;
 	orb_check(_accel_sub, &accel_updated);
@@ -545,9 +528,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_setpoint_poll()
-{
+void FixedwingAttitudeControl::vehicle_setpoint_poll() {
 	/* check if there is a new setpoint */
 	bool att_sp_updated;
 	orb_check(_att_sp_sub, &att_sp_updated);
@@ -558,21 +539,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::global_pos_poll()
-{
+void FixedwingAttitudeControl::global_pos_poll() {
 	/* check if there is a new global position */
 	bool global_pos_updated;
 	orb_check(_global_pos_sub, &global_pos_updated);
 
 	if (global_pos_updated) {
-		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub,
+				&_global_pos);
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_status_poll()
-{
+void FixedwingAttitudeControl::vehicle_status_poll() {
 	/* check if there is new status information */
 	bool vehicle_status_updated;
 	orb_check(_vehicle_status_sub, &vehicle_status_updated);
@@ -582,15 +560,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
-{
+void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) {
 	att_control::g_control->task_main();
 }
 
-void
-FixedwingAttitudeControl::task_main()
-{
+void FixedwingAttitudeControl::task_main() {
 
 	/* inform about start */
 	warnx("Initializing..");
@@ -667,7 +641,6 @@ FixedwingAttitudeControl::task_main()
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
 
-
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
@@ -721,8 +694,8 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */
-				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
-				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s)
+						|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 					if (nonfinite) {
 						perf_count(_nonfinite_input_perf);
@@ -740,16 +713,20 @@ FixedwingAttitudeControl::task_main()
 				 * Forcing the scaling to this value allows reasonable handheld tests.
 				 */
 
-				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
+				float airspeed_scaling = _parameters.airspeed_trim
+						/ ((airspeed < _parameters.airspeed_min) ?
+								_parameters.airspeed_min : airspeed);
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
 				float throttle_sp = 0.0f;
 
-				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+				if (_vcontrol_mode.flag_control_velocity_enabled
+						|| _vcontrol_mode.flag_control_position_enabled) {
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body
+							+ _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -775,10 +752,12 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
-						+ _parameters.rollsp_offset_rad;
-					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
-						+ _parameters.pitchsp_offset_rad;
+					roll_sp = (_manual.y * _parameters.man_roll_max
+							- _parameters.trim_roll)
+							+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max
+							- _parameters.trim_pitch)
+							+ _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -797,11 +776,13 @@ FixedwingAttitudeControl::task_main()
 					/* lazily publish the setpoint only once available */
 					if (_attitude_sp_pub > 0) {
 						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
+						orb_publish(ORB_ID(vehicle_attitude_setpoint),
+								_attitude_sp_pub, &att_sp);
 
 					} else {
 						/* advertise and publish */
-						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+						_attitude_sp_pub = orb_advertise(
+								ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					}
 				}
 
@@ -816,11 +797,17 @@ FixedwingAttitudeControl::task_main()
 				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 = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
-					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
-					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
-				} else	{
+				if (_att.R_valid) {
+					speed_body_u = _att.R[0][0] * _global_pos.vel_n
+							+ _att.R[1][0] * _global_pos.vel_e
+							+ _att.R[2][0] * _global_pos.vel_d;
+					speed_body_v = _att.R[0][1] * _global_pos.vel_n
+							+ _att.R[1][1] * _global_pos.vel_e
+							+ _att.R[2][1] * _global_pos.vel_d;
+					speed_body_w = _att.R[0][2] * _global_pos.vel_n
+							+ _att.R[1][2] * _global_pos.vel_e
+							+ _att.R[2][2] * _global_pos.vel_d;
+				} else {
 					if (_debug && loop_counter % 10 == 0) {
 						warnx("Did not get a valid R\n");
 					}
@@ -829,74 +816,94 @@ FixedwingAttitudeControl::task_main()
 				/* Run attitude controllers */
 				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
 					_roll_ctrl.control_attitude(roll_sp, _att.roll);
-					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll,
+							_att.pitch, airspeed);
 					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
 							speed_body_u, speed_body_v, speed_body_w,
-							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+							_roll_ctrl.get_desired_rate(),
+							_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
 							_att.rollspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
+							_parameters.airspeed_min, _parameters.airspeed_max,
+							airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] =
+							(isfinite(roll_u)) ?
+									roll_u + _parameters.trim_roll :
+									_parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						_roll_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("roll_u %.4f", (double)roll_u);
+							warnx("roll_u %.4f", (double) roll_u);
 						}
 					}
 
-					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
-							_att.pitchspeed, _att.yawspeed,
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll,
+							_att.pitch, _att.pitchspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
+							_parameters.airspeed_min, _parameters.airspeed_max,
+							airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] =
+							(isfinite(pitch_u)) ?
+									pitch_u + _parameters.trim_pitch :
+									_parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
-								" airspeed %.4f, airspeed_scaling %.4f,"
-								" roll_sp %.4f, pitch_sp %.4f,"
-								" _roll_ctrl.get_desired_rate() %.4f,"
-								" _pitch_ctrl.get_desired_rate() %.4f"
-								" att_sp.roll_body %.4f",
-								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
-								(double)airspeed, (double)airspeed_scaling,
-								(double)roll_sp, (double)pitch_sp,
-								(double)_roll_ctrl.get_desired_rate(),
-								(double)_pitch_ctrl.get_desired_rate(),
-								(double)_att_sp.roll_body);
+							warnx(
+									"pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+											" airspeed %.4f, airspeed_scaling %.4f,"
+											" roll_sp %.4f, pitch_sp %.4f,"
+											" _roll_ctrl.get_desired_rate() %.4f,"
+											" _pitch_ctrl.get_desired_rate() %.4f"
+											" att_sp.roll_body %.4f",
+									(double) pitch_u,
+									(double) _yaw_ctrl.get_desired_rate(),
+									(double) airspeed,
+									(double) airspeed_scaling, (double) roll_sp,
+									(double) pitch_sp,
+									(double) _roll_ctrl.get_desired_rate(),
+									(double) _pitch_ctrl.get_desired_rate(),
+									(double) _att_sp.roll_body);
 						}
 					}
 
-					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
-							_att.pitchspeed, _att.yawspeed,
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll,
+							_att.pitch, _att.pitchspeed, _att.yawspeed,
 							_pitch_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+							_parameters.airspeed_min, _parameters.airspeed_max,
+							airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] =
+							(isfinite(yaw_u)) ?
+									yaw_u + _parameters.trim_yaw :
+									_parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("yaw_u %.4f", (double)yaw_u);
+							warnx("yaw_u %.4f", (double) yaw_u);
 						}
 					}
 
 					/* throttle passed through */
-					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					_actuators.control[3] =
+							(isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("throttle_sp %.4f", (double)throttle_sp);
+							warnx("throttle_sp %.4f", (double) throttle_sp);
 						}
 					}
 				} else {
 					perf_count(_nonfinite_input_perf);
 					if (_debug && loop_counter % 10 == 0) {
-						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+						warnx(
+								"Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
+								(double) roll_sp, (double) pitch_sp);
 					}
 				}
 
@@ -913,11 +920,13 @@ FixedwingAttitudeControl::task_main()
 
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub,
+							&rates_sp);
 
 				} else {
 					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),
+							&rates_sp);
 				}
 
 			} else {
@@ -939,16 +948,19 @@ FixedwingAttitudeControl::task_main()
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub,
+						&_actuators);
 
 			} else {
 				/* advertise and publish */
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0),
+						&_actuators);
 			}
 
 			if (_actuators_1_pub > 0) {
 				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub,
+						&_actuators_airframe);
 //				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
 //						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
 //						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
@@ -956,7 +968,8 @@ FixedwingAttitudeControl::task_main()
 
 			} else {
 				/* advertise and publish */
-				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1),
+						&_actuators_airframe);
 			}
 
 		}
@@ -972,18 +985,14 @@ FixedwingAttitudeControl::task_main()
 	_exit(0);
 }
 
-int
-FixedwingAttitudeControl::start()
-{
+int FixedwingAttitudeControl::start() {
 	ASSERT(_control_task == -1);
 
 	/* start the task */
 	_control_task = task_spawn_cmd("fw_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2048,
-				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
-				       nullptr);
+	SCHED_DEFAULT,
+	SCHED_PRIORITY_MAX - 5, 2048,
+			(main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr);
 
 	if (_control_task < 0) {
 		warn("task start failed");
@@ -993,8 +1002,7 @@ FixedwingAttitudeControl::start()
 	return OK;
 }
 
-int fw_att_control_main(int argc, char *argv[])
-{
+int fw_att_control_main(int argc, char *argv[]) {
 	if (argc < 1)
 		errx(1, "usage: fw_att_control {start|stop|status}");
 
@@ -1015,7 +1023,8 @@ int fw_att_control_main(int argc, char *argv[])
 		}
 
 		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
-		while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
+		while (att_control::g_control == nullptr
+				|| !att_control::g_control->task_running()) {
 			usleep(50000);
 			printf(".");
 			fflush(stdout);
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 0f6c9aca17..9ec9d10aba 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -42,8 +42,9 @@
 #define TOPIC_ACTUATOR_ARMED_H
 
 #include <stdint.h>
+#ifdef CONFIG_ARCH_ARM
 #include "../uORB.h"
-
+#endif
 /**
  * @addtogroup topics
  * @{
@@ -64,6 +65,7 @@ struct actuator_armed_s {
  */
 
 /* register this as object request broker structure */
+#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(actuator_armed);
-
+#endif
 #endif

From ac0f01e92e647944b34f7dcf3adbacd283d685cb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 3 Nov 2014 07:41:05 +0100
Subject: [PATCH 024/416] First stab at ROS integration

---
 CMakeLists.txt      | 163 ++++++++++++++++++++++++++++++++++++++++++++
 msg/rc_channels.msg |   8 +++
 package.xml         |  59 ++++++++++++++++
 3 files changed, 230 insertions(+)
 create mode 100644 CMakeLists.txt
 create mode 100644 msg/rc_channels.msg
 create mode 100644 package.xml

diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000..834a8f0c11
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,163 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(px4)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES px4
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+# add_library(px4
+#   src/${PROJECT_NAME}/px4test.cpp # src/platform/ros/ros.cpp
+# )
+
+## Declare a cpp executable
+add_executable(rostest_node src/platform/ros/ros.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(rostest_node px4_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(rostest_node
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS px4 mc_attitude_control
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
new file mode 100644
index 0000000000..9b3c708356
--- /dev/null
+++ b/msg/rc_channels.msg
@@ -0,0 +1,8 @@
+Header header
+int32 RC_CHANNELS_FUNCTION_MAX=18
+uint64 timestamp_last_valid					# Timestamp of last valid RC signal
+float32 channels[RC_CHANNELS_FUNCTION_MAX]			# Scaled to -1..1 (throttle: 0..1)
+uint8 channel_count						# Number of valid channels
+int8 function[RC_CHANNELS_FUNCTION_MAX]				# Functions mapping
+uint8 rssi							# Receive signal strength index
+bool signal_lost						# Control signal lost, should be checked together with topic timeout
\ No newline at end of file
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000000..3575119be7
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package>
+  <name>px4</name>
+  <version>1.0.0</version>
+  <description>The PX4 Flight Stack package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="lorenz@px4.io">Lorenz Meier</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://px4.io/ros</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
\ No newline at end of file

From 3b2b280a41f093020eab1a8a8945603fb3ffacfb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 6 Oct 2014 00:49:25 -0700
Subject: [PATCH 025/416] Get ROS examples to compile, add simple RC channels
 message

---
 CMakeLists.txt                         |  32 ++++--
 msg/rc_channels.msg                    |   6 +-
 src/examples/publisher/publisher.cpp   | 137 +++++++++++++++++++++++++
 src/examples/subscriber/subscriber.cpp |  93 +++++++++++++++++
 4 files changed, 255 insertions(+), 13 deletions(-)
 create mode 100644 src/examples/publisher/publisher.cpp
 create mode 100644 src/examples/subscriber/subscriber.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 834a8f0c11..d370c5a3fc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -44,11 +45,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  rc_channels.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -99,18 +99,30 @@ include_directories(
 
 ## Declare a cpp library
 # add_library(px4
-#   src/${PROJECT_NAME}/px4test.cpp # src/platform/ros/ros.cpp
+#   src/${PROJECT_NAME}/px4test.cpp # src/platforms/ros/ros.cpp
 # )
 
-## Declare a cpp executable
-add_executable(rostest_node src/platform/ros/ros.cpp)
+## Declare a test publisher
+add_executable(publisher src/examples/publisher/publisher.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
-add_dependencies(rostest_node px4_generate_messages_cpp)
+add_dependencies(publisher px4_generate_messages_cpp)
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(rostest_node
+target_link_libraries(publisher
+  ${catkin_LIBRARIES}
+)
+
+## Declare a test subscriber
+add_executable(subscriber src/examples/subscriber/subscriber.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(subscriber px4_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(subscriber
   ${catkin_LIBRARIES}
 )
 
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index 9b3c708356..11dc57c42b 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -1,8 +1,8 @@
 Header header
 int32 RC_CHANNELS_FUNCTION_MAX=18
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
-float32 channels[RC_CHANNELS_FUNCTION_MAX]			# Scaled to -1..1 (throttle: 0..1)
+float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8 function[RC_CHANNELS_FUNCTION_MAX]				# Functions mapping
+int8[18] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
-bool signal_lost						# Control signal lost, should be checked together with topic timeout
\ No newline at end of file
+bool signal_lost						# Control signal lost, should be checked together with topic timeout
diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
new file mode 100644
index 0000000000..2c0f844767
--- /dev/null
+++ b/src/examples/publisher/publisher.cpp
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the names of Stanford University or Willow Garage, Inc. 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.
+ */
+// %Tag(FULLTEXT)%
+// %Tag(ROS_HEADER)%
+#include "ros/ros.h"
+// %EndTag(ROS_HEADER)%
+// %Tag(MSG_HEADER)%
+#include "std_msgs/String.h"
+// %EndTag(MSG_HEADER)%
+
+#include <sstream>
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+int main(int argc, char **argv)
+{
+  /**
+   * The ros::init() function needs to see argc and argv so that it can perform
+   * any ROS arguments and name remapping that were provided at the command line. For programmatic
+   * remappings you can use a different version of init() which takes remappings
+   * directly, but for most command-line programs, passing argc and argv is the easiest
+   * way to do it.  The third argument to init() is the name of the node.
+   *
+   * You must call one of the versions of ros::init() before using any other
+   * part of the ROS system.
+   */
+// %Tag(INIT)%
+  ros::init(argc, argv, "talker");
+// %EndTag(INIT)%
+
+  /**
+   * NodeHandle is the main access point to communications with the ROS system.
+   * The first NodeHandle constructed will fully initialize this node, and the last
+   * NodeHandle destructed will close down the node.
+   */
+// %Tag(NODEHANDLE)%
+  ros::NodeHandle n;
+// %EndTag(NODEHANDLE)%
+
+  /**
+   * The advertise() function is how you tell ROS that you want to
+   * publish on a given topic name. This invokes a call to the ROS
+   * master node, which keeps a registry of who is publishing and who
+   * is subscribing. After this advertise() call is made, the master
+   * node will notify anyone who is trying to subscribe to this topic name,
+   * and they will in turn negotiate a peer-to-peer connection with this
+   * node.  advertise() returns a Publisher object which allows you to
+   * publish messages on that topic through a call to publish().  Once
+   * all copies of the returned Publisher object are destroyed, the topic
+   * will be automatically unadvertised.
+   *
+   * The second parameter to advertise() is the size of the message queue
+   * used for publishing messages.  If messages are published more quickly
+   * than we can send them, the number here specifies how many messages to
+   * buffer up before throwing some away.
+   */
+// %Tag(PUBLISHER)%
+  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
+// %EndTag(PUBLISHER)%
+
+// %Tag(LOOP_RATE)%
+  ros::Rate loop_rate(10);
+// %EndTag(LOOP_RATE)%
+
+  /**
+   * A count of how many messages we have sent. This is used to create
+   * a unique string for each message.
+   */
+// %Tag(ROS_OK)%
+  int count = 0;
+  while (ros::ok())
+  {
+// %EndTag(ROS_OK)%
+    /**
+     * This is a message object. You stuff it with data, and then publish it.
+     */
+// %Tag(FILL_MESSAGE)%
+    std_msgs::String msg;
+
+    std::stringstream ss;
+    ss << "hello world " << count;
+    msg.data = ss.str();
+// %EndTag(FILL_MESSAGE)%
+
+// %Tag(ROSCONSOLE)%
+    ROS_INFO("%s", msg.data.c_str());
+// %EndTag(ROSCONSOLE)%
+
+    /**
+     * The publish() function is how you send messages. The parameter
+     * is the message object. The type of this object must agree with the type
+     * given as a template parameter to the advertise<>() call, as was done
+     * in the constructor above.
+     */
+// %Tag(PUBLISH)%
+    chatter_pub.publish(msg);
+// %EndTag(PUBLISH)%
+
+// %Tag(SPINONCE)%
+    ros::spinOnce();
+// %EndTag(SPINONCE)%
+
+// %Tag(RATE_SLEEP)%
+    loop_rate.sleep();
+// %EndTag(RATE_SLEEP)%
+    ++count;
+  }
+
+
+  return 0;
+}
+// %EndTag(FULLTEXT)%
\ No newline at end of file
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
new file mode 100644
index 0000000000..e46055306c
--- /dev/null
+++ b/src/examples/subscriber/subscriber.cpp
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the names of Stanford University or Willow Garage, Inc. 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.
+ */
+
+// %Tag(FULLTEXT)%
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the ROS system.
+ */
+// %Tag(CALLBACK)%
+void chatterCallback(const std_msgs::String::ConstPtr& msg)
+{
+  ROS_INFO("I heard: [%s]", msg->data.c_str());
+}
+// %EndTag(CALLBACK)%
+
+int main(int argc, char **argv)
+{
+  /**
+   * The ros::init() function needs to see argc and argv so that it can perform
+   * any ROS arguments and name remapping that were provided at the command line. For programmatic
+   * remappings you can use a different version of init() which takes remappings
+   * directly, but for most command-line programs, passing argc and argv is the easiest
+   * way to do it.  The third argument to init() is the name of the node.
+   *
+   * You must call one of the versions of ros::init() before using any other
+   * part of the ROS system.
+   */
+  ros::init(argc, argv, "listener");
+
+  /**
+   * NodeHandle is the main access point to communications with the ROS system.
+   * The first NodeHandle constructed will fully initialize this node, and the last
+   * NodeHandle destructed will close down the node.
+   */
+  ros::NodeHandle n;
+
+  /**
+   * The subscribe() call is how you tell ROS that you want to receive messages
+   * on a given topic.  This invokes a call to the ROS
+   * master node, which keeps a registry of who is publishing and who
+   * is subscribing.  Messages are passed to a callback function, here
+   * called chatterCallback.  subscribe() returns a Subscriber object that you
+   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
+   * object go out of scope, this callback will automatically be unsubscribed from
+   * this topic.
+   *
+   * The second parameter to the subscribe() function is the size of the message
+   * queue.  If messages are arriving faster than they are being processed, this
+   * is the number of messages that will be buffered up before beginning to throw
+   * away the oldest ones.
+   */
+// %Tag(SUBSCRIBER)%
+  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
+// %EndTag(SUBSCRIBER)%
+
+  /**
+   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
+   * callbacks will be called from within this thread (the main one).  ros::spin()
+   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
+   */
+// %Tag(SPIN)%
+  ros::spin();
+// %EndTag(SPIN)%
+
+  return 0;
+}
+// %EndTag(FULLTEXT)%
\ No newline at end of file

From 3956a2c836df9aa411815b4a4027fdcfb864f041 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 6 Oct 2014 08:12:41 -0700
Subject: [PATCH 026/416] Baby steps towards PX4 ROS nodes

---
 CMakeLists.txt                       |  4 +--
 include/px4.h                        | 47 ++++++++++++++++++++++++++++
 src/examples/publisher/publisher.cpp | 10 +++---
 3 files changed, 54 insertions(+), 7 deletions(-)
 create mode 100644 include/px4.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d370c5a3fc..144bdcbc34 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,7 +80,7 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS include
 #  LIBRARIES px4
 #  CATKIN_DEPENDS roscpp rospy std_msgs
 #  DEPENDS system_lib
@@ -92,7 +92,7 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
+include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
diff --git a/include/px4.h b/include/px4.h
new file mode 100644
index 0000000000..7dedd5f822
--- /dev/null
+++ b/include/px4.h
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4.h
+ *
+ * Main system header with common convenience functions
+ */
+
+#ifdef __linux
+#include "ros/ros.h"
+#define px4_warnx ROS_WARN
+#define px4_infox ROS_INFO
+#else
+#define px4_warnx warnx
+#define px4_infox warnx
+#endif
\ No newline at end of file
diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 2c0f844767..cc88cd543d 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -24,9 +24,9 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-// %Tag(FULLTEXT)%
-// %Tag(ROS_HEADER)%
-#include "ros/ros.h"
+
+#include <px4.h>
+
 // %EndTag(ROS_HEADER)%
 // %Tag(MSG_HEADER)%
 #include "std_msgs/String.h"
@@ -50,7 +50,7 @@ int main(int argc, char **argv)
    * part of the ROS system.
    */
 // %Tag(INIT)%
-  ros::init(argc, argv, "talker");
+  ros::init(argc, argv, "px4_publisher");
 // %EndTag(INIT)%
 
   /**
@@ -108,7 +108,7 @@ int main(int argc, char **argv)
 // %EndTag(FILL_MESSAGE)%
 
 // %Tag(ROSCONSOLE)%
-    ROS_INFO("%s", msg.data.c_str());
+    px4_warnx("%s", msg.data.c_str());
 // %EndTag(ROSCONSOLE)%
 
     /**

From 4fdf8e1ff26d37513c003ea1e92445fae81cc2cb Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 23 Oct 2014 16:59:21 +0200
Subject: [PATCH 027/416] updated from remote

---
 ROMFS/px4fmu_common/init.d/rc.vtol_apps       |  15 +
 ROMFS/px4fmu_common/init.d/rc.vtol_defaults   |  63 ++
 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix   |  15 +
 Tools/tests-host/sf0x_test.cpp                |  65 ++
 Tools/tests-host/st24_test.cpp                |  76 ++
 src/drivers/sf0x/sf0x_parser.cpp              | 155 +++
 src/drivers/sf0x/sf0x_parser.h                |  51 +
 src/lib/rc/module.mk                          |  40 +
 src/lib/rc/st24.c                             | 253 +++++
 src/lib/rc/st24.h                             | 163 ++++
 src/modules/bottle_drop/bottle_drop.cpp       | 907 ++++++++++++++++++
 src/modules/bottle_drop/bottle_drop_params.c  | 131 +++
 src/modules/bottle_drop/module.mk             |  41 +
 .../mc_att_control/mc_att_control_base.cpp    | 283 ++++++
 .../mc_att_control/mc_att_control_base.h      | 172 ++++
 src/modules/navigator/datalinkloss.cpp        | 227 +++++
 src/modules/navigator/datalinkloss.h          |  98 ++
 src/modules/navigator/datalinkloss_params.c   | 126 +++
 src/modules/navigator/enginefailure.cpp       | 149 +++
 src/modules/navigator/enginefailure.h         |  83 ++
 src/modules/navigator/gpsfailure.cpp          | 178 ++++
 src/modules/navigator/gpsfailure.h            |  97 ++
 src/modules/navigator/gpsfailure_params.c     |  97 ++
 src/modules/navigator/rcloss.cpp              | 182 ++++
 src/modules/navigator/rcloss.h                |  88 ++
 src/modules/navigator/rcloss_params.c         |  60 ++
 .../uORB/topics/multirotor_motor_limits.h     |  69 ++
 src/modules/uORB/topics/vtol_vehicle_status.h |  66 ++
 src/modules/vtol_att_control/module.mk        |  40 +
 .../vtol_att_control_main.cpp                 | 642 +++++++++++++
 30 files changed, 4632 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps
 create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults
 create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix
 create mode 100644 Tools/tests-host/sf0x_test.cpp
 create mode 100644 Tools/tests-host/st24_test.cpp
 create mode 100644 src/drivers/sf0x/sf0x_parser.cpp
 create mode 100644 src/drivers/sf0x/sf0x_parser.h
 create mode 100644 src/lib/rc/module.mk
 create mode 100644 src/lib/rc/st24.c
 create mode 100644 src/lib/rc/st24.h
 create mode 100644 src/modules/bottle_drop/bottle_drop.cpp
 create mode 100644 src/modules/bottle_drop/bottle_drop_params.c
 create mode 100644 src/modules/bottle_drop/module.mk
 create mode 100644 src/modules/mc_att_control/mc_att_control_base.cpp
 create mode 100644 src/modules/mc_att_control/mc_att_control_base.h
 create mode 100644 src/modules/navigator/datalinkloss.cpp
 create mode 100644 src/modules/navigator/datalinkloss.h
 create mode 100644 src/modules/navigator/datalinkloss_params.c
 create mode 100644 src/modules/navigator/enginefailure.cpp
 create mode 100644 src/modules/navigator/enginefailure.h
 create mode 100644 src/modules/navigator/gpsfailure.cpp
 create mode 100644 src/modules/navigator/gpsfailure.h
 create mode 100644 src/modules/navigator/gpsfailure_params.c
 create mode 100644 src/modules/navigator/rcloss.cpp
 create mode 100644 src/modules/navigator/rcloss.h
 create mode 100644 src/modules/navigator/rcloss_params.c
 create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h
 create mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h
 create mode 100644 src/modules/vtol_att_control/module.mk
 create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp

diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
new file mode 100644
index 0000000000..23ade6d78b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
@@ -0,0 +1,15 @@
+#!nsh
+#
+# Standard apps for vtol:
+# att & pos estimator, att & pos control.
+#
+
+attitude_estimator_ekf start
+#ekf_att_pos_estimator start
+position_estimator_inav start
+
+vtol_att_control start
+mc_att_control start
+mc_pos_control start
+fw_att_control start
+fw_pos_control_l1 start
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
new file mode 100644
index 0000000000..f0ea9add07
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
@@ -0,0 +1,63 @@
+#!nsh
+
+set VEHICLE_TYPE vtol
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#Default parameters for MC
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	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 MC_YAW_FF 0.5
+
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
+	param set MPC_LAND_SPEED 1.0
+
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.7
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.0
+
+	param set NAV_ACCEPT_RAD 2.0
+	
+	#
+	# Default parameters for FW
+	#
+	param set NAV_LAND_ALT 90
+	param set NAV_RTL_ALT 100
+	param set NAV_RTL_LAND_T -1
+	param set NAV_ACCEPT_RAD 50
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
+fi
+
+set PWM_RATE 400
+set PWM_DISARMED 900
+set PWM_MIN 1075
+set PWM_MAX 2000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix
new file mode 100644
index 0000000000..b077ff30ac
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix
@@ -0,0 +1,15 @@
+#!nsh
+#Quadshot mixer for PX4FMU
+#===========================
+R: 4v 10000 10000 10000 0
+
+#mixer for the elevons
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0   7500   7500      0 -10000  10000
+S: 1 1   8000   8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0   7500   7500      0 -10000  10000
+S: 1 1  -8000  -8000      0 -10000  10000
diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp
new file mode 100644
index 0000000000..82d19fcbe1
--- /dev/null
+++ b/Tools/tests-host/sf0x_test.cpp
@@ -0,0 +1,65 @@
+
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/sf0x/sf0x_parser.h>
+
+int main(int argc, char *argv[])
+{
+	warnx("SF0X test started");
+
+	int ret = 0;
+
+	const char LINE_MAX = 20;
+	char _linebuf[LINE_MAX];
+	_linebuf[0] = '\0';
+
+	const char *lines[] = {"0.01\r\n",
+			       "0.02\r\n",
+			       "0.03\r\n",
+			       "0.04\r\n",
+			       "0",
+			       ".",
+			       "0",
+			       "5",
+			       "\r",
+			       "\n",
+			       "0",
+			       "3\r",
+			       "\n"
+			       "\r\n",
+			       "0.06",
+			       "\r\n"
+			      };
+
+	enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
+	float dist_m;
+	char _parserbuf[LINE_MAX];
+	unsigned _parsebuf_index = 0;
+
+	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
+
+		printf("\n%s", _linebuf);
+
+		int parse_ret;
+
+		for (int i = 0; i < strlen(lines[l]); i++) {
+			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
+
+			if (parse_ret == 0) {
+				printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
+			}
+		}
+
+		printf("%s", lines[l]);
+
+	}
+
+	warnx("test finished");
+
+	return ret;
+}
diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp
new file mode 100644
index 0000000000..25a9355e2e
--- /dev/null
+++ b/Tools/tests-host/st24_test.cpp
@@ -0,0 +1,76 @@
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <rc/st24.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[])
+{
+	warnx("ST24 test started");
+
+	if (argc < 2) {
+		errx(1, "Need a filename for the input file");
+	}
+
+	warnx("loading data from: %s", argv[1]);
+
+	FILE *fp;
+
+	fp = fopen(argv[1], "rt");
+
+	if (!fp) {
+		errx(1, "failed opening file");
+	}
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[20];
+
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
+			warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+
+				int16_t val = channels[i];
+				warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	if (ret == EOF) {
+		warnx("Test finished, reached end of file");
+
+	} else {
+		warnx("Test aborted, errno: %d", ret);
+	}
+
+}
diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp
new file mode 100644
index 0000000000..8e73b0ad35
--- /dev/null
+++ b/src/drivers/sf0x/sf0x_parser.cpp
@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include "sf0x_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+//#define SF0X_DEBUG
+
+#ifdef SF0X_DEBUG
+#include <stdio.h>
+
+const char *parser_state[] = {
+	"0_UNSYNC",
+	"1_SYNC",
+	"2_GOT_DIGIT0",
+	"3_GOT_DOT",
+	"4_GOT_DIGIT1",
+	"5_GOT_DIGIT2",
+	"6_GOT_CARRIAGE_RETURN"
+};
+#endif
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
+{
+	int ret = -1;
+	char *end;
+
+	switch (*state) {
+	case SF0X_PARSE_STATE0_UNSYNC:
+		if (c == '\n') {
+			*state = SF0X_PARSE_STATE1_SYNC;
+			(*parserbuf_index) = 0;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE1_SYNC:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE2_GOT_DIGIT0:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else if (c == '.') {
+			*state = SF0X_PARSE_STATE3_GOT_DOT;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE3_GOT_DOT:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE4_GOT_DIGIT1:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE5_GOT_DIGIT2:
+		if (c == '\r') {
+			*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
+		if (c == '\n') {
+			parserbuf[*parserbuf_index] = '\0';
+			*dist = strtod(parserbuf, &end);
+			*state = SF0X_PARSE_STATE1_SYNC;
+			*parserbuf_index = 0;
+			ret = 0;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+	}
+
+#ifdef SF0X_DEBUG
+	printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
+#endif
+
+	return ret;
+}
\ No newline at end of file
diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h
new file mode 100644
index 0000000000..20892d50eb
--- /dev/null
+++ b/src/drivers/sf0x/sf0x_parser.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Declarations of parser for the Lightware SF0x laser rangefinder series
+ */
+
+enum SF0X_PARSE_STATE {
+	SF0X_PARSE_STATE0_UNSYNC = 0,
+	SF0X_PARSE_STATE1_SYNC,
+	SF0X_PARSE_STATE2_GOT_DIGIT0,
+	SF0X_PARSE_STATE3_GOT_DOT,
+	SF0X_PARSE_STATE4_GOT_DIGIT1,
+	SF0X_PARSE_STATE5_GOT_DIGIT2,
+	SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
+};
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
\ No newline at end of file
diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk
new file mode 100644
index 0000000000..e089c69658
--- /dev/null
+++ b/src/lib/rc/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Yuntec ST24 transmitter protocol decoder
+#
+
+SRCS		 =	st24.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c
new file mode 100644
index 0000000000..e8a791b8f6
--- /dev/null
+++ b/src/lib/rc/st24.c
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file st24.h
+ *
+ * RC protocol implementation for Yuneec ST24 transmitter.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <stdbool.h>
+#include <stdio.h>
+#include "st24.h"
+
+enum ST24_DECODE_STATE {
+	ST24_DECODE_STATE_UNSYNCED = 0,
+	ST24_DECODE_STATE_GOT_STX1,
+	ST24_DECODE_STATE_GOT_STX2,
+	ST24_DECODE_STATE_GOT_LEN,
+	ST24_DECODE_STATE_GOT_TYPE,
+	ST24_DECODE_STATE_GOT_DATA
+};
+
+const char *decode_states[] = {"UNSYNCED",
+			       "GOT_STX1",
+			       "GOT_STX2",
+			       "GOT_LEN",
+			       "GOT_TYPE",
+			       "GOT_DATA"
+			      };
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define ST24_RANGE_MIN 0.0f
+#define ST24_RANGE_MAX 4096.0f
+
+#define ST24_TARGET_MIN 1000.0f
+#define ST24_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
+#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
+
+static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
+static unsigned _rxlen;
+
+static ReceiverFcPacket _rxpacket;
+
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
+{
+	uint8_t i, crc ;
+	crc = 0;
+
+	while (len--) {
+		for (i = 0x80; i != 0; i >>= 1) {
+			if ((crc & 0x80) != 0) {
+				crc <<= 1;
+				crc ^= 0x07;
+
+			} else {
+				crc <<= 1;
+			}
+
+			if ((*ptr & i) != 0) {
+				crc ^= 0x07;
+			}
+		}
+
+		ptr++;
+	}
+
+	return (crc);
+}
+
+
+int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
+		    uint16_t max_chan_count)
+{
+
+	int ret = 1;
+
+	switch (_decode_state) {
+	case ST24_DECODE_STATE_UNSYNCED:
+		if (byte == ST24_STX1) {
+			_decode_state = ST24_DECODE_STATE_GOT_STX1;
+		} else {
+			ret = 3;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_STX1:
+		if (byte == ST24_STX2) {
+			_decode_state = ST24_DECODE_STATE_GOT_STX2;
+
+		} else {
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_STX2:
+
+		/* ensure no data overflow failure or hack is possible */
+		if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
+			_rxpacket.length = byte;
+			_rxlen = 0;
+			_decode_state = ST24_DECODE_STATE_GOT_LEN;
+
+		} else {
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_LEN:
+		_rxpacket.type = byte;
+		_rxlen++;
+		_decode_state = ST24_DECODE_STATE_GOT_TYPE;
+		break;
+
+	case ST24_DECODE_STATE_GOT_TYPE:
+		_rxpacket.st24_data[_rxlen - 1] = byte;
+		_rxlen++;
+
+		if (_rxlen == (_rxpacket.length - 1)) {
+			_decode_state = ST24_DECODE_STATE_GOT_DATA;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_DATA:
+		_rxpacket.crc8 = byte;
+		_rxlen++;
+
+		if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
+
+			ret = 0;
+
+			/* decode the actual packet */
+
+			switch (_rxpacket.type) {
+
+			case ST24_PACKET_TYPE_CHANNELDATA12: {
+					ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
+
+					*rssi = d->rssi;
+					*rx_count = d->packet_count;
+
+					/* this can lead to rounding of the strides */
+					*channel_count = (max_chan_count < 12) ? max_chan_count : 12;
+
+					unsigned stride_count = (*channel_count * 3) / 2;
+					unsigned chan_index = 0;
+
+					for (unsigned i = 0; i < stride_count; i += 3) {
+						channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+						channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+
+						channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+						channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+					}
+				}
+				break;
+
+			case ST24_PACKET_TYPE_CHANNELDATA24: {
+					ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
+
+					*rssi = d->rssi;
+					*rx_count = d->packet_count;
+
+					/* this can lead to rounding of the strides */
+					*channel_count = (max_chan_count < 24) ? max_chan_count : 24;
+
+					unsigned stride_count = (*channel_count * 3) / 2;
+					unsigned chan_index = 0;
+
+					for (unsigned i = 0; i < stride_count; i += 3) {
+						channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+						channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+
+						channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+						channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+					}
+				}
+				break;
+
+			case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
+
+					// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
+					/* we silently ignore this data for now, as it is unused */
+					ret = 2;
+				}
+				break;
+
+			default:
+				ret = 2;
+				break;
+			}
+
+		} else {
+			/* decoding failed */
+			ret = 4;
+		}
+
+		_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		break;
+	}
+
+	return ret;
+}
diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h
new file mode 100644
index 0000000000..454234601d
--- /dev/null
+++ b/src/lib/rc/st24.h
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *	Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *	notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *	notice, this list of conditions and the following disclaimer in
+ *	the documentation and/or other materials provided with the
+ *	distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *	used to endorse or promote products derived from this software
+ *	without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file st24.h
+ *
+ * RC protocol definition for Yuneec ST24 transmitter
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+#define ST24_DATA_LEN_MAX	64
+#define ST24_STX1		0x55
+#define ST24_STX2		0x55
+
+enum ST24_PACKET_TYPE {
+	ST24_PACKET_TYPE_CHANNELDATA12 = 0,
+	ST24_PACKET_TYPE_CHANNELDATA24,
+	ST24_PACKET_TYPE_TRANSMITTERGPSDATA
+};
+
+#pragma pack(push, 1)
+typedef struct {
+	uint8_t	header1;			///< 0x55 for a valid packet
+	uint8_t	header2;			///< 0x55 for a valid packet
+	uint8_t	length;				///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
+	uint8_t	type;				///< from enum ST24_PACKET_TYPE
+	uint8_t	st24_data[ST24_DATA_LEN_MAX];
+	uint8_t	crc8;				///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
+} ReceiverFcPacket;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ * This is incoming from the ST24
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
+	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+	uint8_t	channel[18];		///< channel data, 12 channels (12 bit numbers)
+} ChannelData12;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
+	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+	uint8_t	channel[36];		///< channel data, 24 channels (12 bit numbers)
+} ChannelData24;
+
+/**
+ * Telemetry packet
+ *
+ * This is outgoing to the ST24
+ *
+ * imuStatus:
+ * 8 bit total
+ * bits 0-2 for status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - values 3 through 7 are reserved
+ * bits 3-7 are status for sensors (0 or 1)
+ * - mpu6050
+ * - accelerometer
+ * - primary gyro x
+ * - primary gyro y
+ * - primary gyro z
+ *
+ * pressCompassStatus
+ * 8 bit total
+ * bits 0-3 for compass status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ * bits 4-7 for pressure status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ *
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	int32_t	lat;			///< lattitude (degrees)	+/- 90 deg
+	int32_t	lon;			///< longitude (degrees)	+/- 180 deg
+	int32_t	alt;			///< 0.01m resolution, altitude (meters)
+	int16_t	vx, vy, vz; 		///< velocity 0.01m res, +/-320.00 North-East- Down
+	uint8_t	nsat;			///<number of satellites
+	uint8_t	voltage; 		///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
+	uint8_t	current; 		///< 0.5A resolution
+	int16_t	roll, pitch, yaw;	///< 0.01 degree resolution
+	uint8_t	motorStatus;		///< 1 bit per motor for status 1=good, 0= fail
+	uint8_t	imuStatus;		///< inertial measurement unit status
+	uint8_t	pressCompassStatus;	///< baro / compass status
+} TelemetryData;
+
+#pragma pack(pop)
+
+/**
+ * CRC8 implementation for ST24 protocol
+ *
+ * @param prt Pointer to the data to CRC
+ * @param len number of bytes to accumulate in the checksum
+ * @return the checksum of these bytes over len
+ */
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
+
+/**
+ * Decoder for ST24 protocol
+ *
+ * @param byte current char to read
+ * @param rssi pointer to a byte where the RSSI value is written back to
+ * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
+ * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
+ * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
+ * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
+ */
+__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+			     uint16_t *channels, uint16_t max_chan_count);
+
+__END_DECLS
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
new file mode 100644
index 0000000000..31c9157e11
--- /dev/null
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -0,0 +1,907 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bottle_drop.cpp
+ *
+ * Bottle drop module for Outback Challenge 2014, Team Swiss Fang
+ *
+ * @author Dominik Juchli <juchlid@ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <sys/ioctl.h>
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <dataman/dataman.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+
+/**
+ * bottle_drop app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]);
+
+class BottleDrop
+{
+public:
+	/**
+	 * Constructor
+	 */
+	BottleDrop();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~BottleDrop();
+
+	/**
+	 * Start the task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Display status.
+	 */
+	void		status();
+
+	void		open_bay();
+	void		close_bay();
+	void		drop();
+	void		lock_release();
+
+private:
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_main_task;			/**< handle for task */
+	int		_mavlink_fd;
+
+	int		_command_sub;
+	int		_wind_estimate_sub;
+	struct vehicle_command_s	_command;
+	struct vehicle_global_position_s _global_pos;
+	map_projection_reference_s ref;
+
+	orb_advert_t	_actuator_pub;
+	struct actuator_controls_s _actuators;
+
+	bool		_drop_approval;
+	hrt_abstime	_doors_opened;
+	hrt_abstime	_drop_time;
+
+	float		_alt_clearance;
+
+	struct position_s {
+		double lat;	///< degrees
+		double lon;	///< degrees
+		float alt;	///< m
+	} _target_position, _drop_position;
+
+	enum DROP_STATE {
+		DROP_STATE_INIT = 0,
+		DROP_STATE_TARGET_VALID,
+		DROP_STATE_TARGET_SET,
+		DROP_STATE_BAY_OPEN,
+		DROP_STATE_DROPPED,
+		DROP_STATE_BAY_CLOSED
+	} _drop_state;
+
+	struct mission_s	_onboard_mission;
+	orb_advert_t		_onboard_mission_pub;
+
+	void		task_main();
+
+	void		handle_command(struct vehicle_command_s *cmd);
+
+	void		answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result);
+
+	/**
+	 * Set the actuators
+	 */
+	int		actuators_publish();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+};
+
+namespace bottle_drop
+{
+BottleDrop	*g_bottle_drop;
+}
+
+BottleDrop::BottleDrop() :
+
+	_task_should_exit(false),
+	_main_task(-1),
+	_mavlink_fd(-1),
+	_command_sub(-1),
+	_wind_estimate_sub(-1),
+	_command {},
+	_global_pos {},
+	ref {},
+	_actuator_pub(-1),
+	_actuators {},
+	_drop_approval(false),
+	_doors_opened(0),
+	_drop_time(0),
+	_alt_clearance(70.0f),
+	_target_position {},
+	_drop_position {},
+	_drop_state(DROP_STATE_INIT),
+	_onboard_mission {},
+	_onboard_mission_pub(-1)
+{
+}
+
+BottleDrop::~BottleDrop()
+{
+	if (_main_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_main_task);
+				break;
+			}
+		} while (_main_task != -1);
+	}
+
+	bottle_drop::g_bottle_drop = nullptr;
+}
+
+int
+BottleDrop::start()
+{
+	ASSERT(_main_task == -1);
+
+	/* start the task */
+	_main_task = task_spawn_cmd("bottle_drop",
+				    SCHED_DEFAULT,
+				    SCHED_PRIORITY_DEFAULT + 15,
+				    2048,
+				    (main_t)&BottleDrop::task_main_trampoline,
+				    nullptr);
+
+	if (_main_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+
+void
+BottleDrop::status()
+{
+	warnx("drop state: %d", _drop_state);
+}
+
+void
+BottleDrop::open_bay()
+{
+	_actuators.control[0] = -1.0f;
+	_actuators.control[1] = 1.0f;
+
+	if (_doors_opened == 0) {
+		_doors_opened = hrt_absolute_time();
+	}
+	warnx("open doors");
+
+	actuators_publish();
+
+	usleep(500 * 1000);
+}
+
+void
+BottleDrop::close_bay()
+{
+	// closed door and locked survival kit
+	_actuators.control[0] = 1.0f;
+	_actuators.control[1] = -1.0f;
+
+	_doors_opened = 0;
+
+	actuators_publish();
+
+	// delay until the bay is closed
+	usleep(500 * 1000);
+}
+
+void
+BottleDrop::drop()
+{
+
+	// update drop actuator, wait 0.5s until the doors are open before dropping
+	hrt_abstime starttime = hrt_absolute_time();
+
+	// force the door open if we have to
+	if (_doors_opened == 0) {
+		open_bay();
+		warnx("bay not ready, forced open");
+	}
+
+	while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
+		usleep(50000);
+		warnx("delayed by door!");
+	}
+
+	_actuators.control[2] = 1.0f;
+
+	_drop_time = hrt_absolute_time();
+	actuators_publish();
+
+	warnx("dropping now");
+
+	// Give it time to drop
+	usleep(1000 * 1000);
+}
+
+void
+BottleDrop::lock_release()
+{
+	_actuators.control[2] = -1.0f;
+	actuators_publish();
+
+	warnx("closing release");
+}
+
+int
+BottleDrop::actuators_publish()
+{
+	_actuators.timestamp = hrt_absolute_time();
+
+	// lazily publish _actuators only once available
+	if (_actuator_pub > 0) {
+		return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
+
+	} else {
+		_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
+		if (_actuator_pub > 0) {
+			return OK;
+		} else {
+			return -1;
+		}
+	}
+}
+
+void
+BottleDrop::task_main()
+{
+
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
+
+	_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
+
+	bool updated = false;
+
+	float z_0;		// ground properties
+	float turn_radius;	// turn radius of the UAV
+	float precision;	// Expected precision of the UAV
+
+	float ground_distance = _alt_clearance;		// Replace by closer estimate in loop
+
+	// constant
+	float g = CONSTANTS_ONE_G;               		// constant of gravity [m/s^2]
+	float m = 0.5f;                		// mass of bottle [kg]
+	float rho = 1.2f;              		// air density [kg/m^3]
+	float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
+	float dt_freefall_prediction = 0.01f;   // step size of the free fall prediction [s]
+
+	// Has to be estimated by experiment
+	float cd = 0.86f;              	// Drag coefficient for a cylinder with a d/l ratio of 1/3 []
+	float t_signal =
+		0.084f;	// Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
+	float t_door =
+		0.7f;		// The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
+
+
+	// Definition
+	float h_0;						// height over target
+	float az;                 				// acceleration in z direction[m/s^2]
+	float vz; 						// velocity in z direction [m/s]
+	float z; 						// fallen distance [m]
+	float h; 						// height over target [m]
+	float ax;						// acceleration in x direction [m/s^2]
+	float vx;						// ground speed in x direction [m/s]
+	float x;					        // traveled distance in x direction [m]
+	float vw;                 				// wind speed [m/s]
+	float vrx;						// relative velocity in x direction [m/s]
+	float v;						// relative speed vector [m/s]
+	float Fd;						// Drag force [N]
+	float Fdx;						// Drag force in x direction [N]
+	float Fdz;						// Drag force in z direction [N]
+	float x_drop, y_drop;					// coordinates of the drop point in reference to the target (projection of NED)
+	float x_t, y_t;						// coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
+	float x_l, y_l;						// local position in projected coordinates
+	float x_f, y_f;						// to-be position of the UAV after dt_runs seconds in projected coordinates
+	double x_f_NED, y_f_NED;				// to-be position of the UAV after dt_runs seconds in NED
+	float distance_open_door;				// The distance the UAV travels during its doors open [m]
+	float approach_error = 0.0f;				// The error in radians between current ground vector and desired ground vector
+	float distance_real = 0;				// The distance between the UAVs position and the drop point [m]
+	float future_distance = 0;				// The distance between the UAVs to-be position and the drop point [m]
+
+	unsigned counter = 0;
+
+	param_t param_gproperties = param_find("BD_GPROPERTIES");
+	param_t param_turn_radius = param_find("BD_TURNRADIUS");
+	param_t param_precision = param_find("BD_PRECISION");
+	param_t param_cd = param_find("BD_OBJ_CD");
+	param_t param_mass = param_find("BD_OBJ_MASS");
+	param_t param_surface = param_find("BD_OBJ_SURFACE");
+
+
+	param_get(param_precision, &precision);
+	param_get(param_turn_radius, &turn_radius);
+	param_get(param_gproperties, &z_0);
+	param_get(param_cd, &cd);
+	param_get(param_mass, &m);
+	param_get(param_surface, &A);
+
+	int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+
+	struct parameter_update_s update;
+	memset(&update, 0, sizeof(update));
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	struct mission_item_s flight_vector_s {};
+	struct mission_item_s flight_vector_e {};
+
+	flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
+	flight_vector_s.acceptance_radius = 50; // TODO: make parameter
+	flight_vector_s.autocontinue = true;
+	flight_vector_s.altitude_is_relative = false;
+
+	flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
+	flight_vector_e.acceptance_radius = 50; // TODO: make parameter
+	flight_vector_e.autocontinue = true;
+	flight_vector_s.altitude_is_relative = false;
+
+	struct wind_estimate_s wind;
+
+	// wakeup source(s)
+	struct pollfd fds[1];
+
+	// Setup of loop
+	fds[0].fd = _command_sub;
+	fds[0].events = POLLIN;
+
+	// Whatever state the bay is in, we want it closed on startup
+	lock_release();
+	close_bay();
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		/* vehicle commands updated */
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
+			handle_command(&_command);
+		}
+
+		orb_check(vehicle_global_position_sub, &updated);
+		if (updated) {
+			/* copy global position */
+			orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
+		}
+
+		if (_global_pos.timestamp == 0) {
+			continue;
+		}
+
+		const unsigned sleeptime_us = 9500;
+
+		hrt_abstime last_run = hrt_absolute_time();
+		float dt_runs = sleeptime_us / 1e6f;
+
+		// switch to faster updates during the drop
+		while (_drop_state > DROP_STATE_INIT) {
+
+			// Get wind estimate
+			orb_check(_wind_estimate_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
+			}
+
+			// Get vehicle position
+			orb_check(vehicle_global_position_sub, &updated);
+			if (updated) {
+				// copy global position
+				orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
+			}
+
+			// Get parameter updates
+			orb_check(parameter_update_sub, &updated);
+			if (updated) {
+				// copy global position
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
+
+				// update all parameters
+				param_get(param_gproperties, &z_0);
+				param_get(param_turn_radius, &turn_radius);
+				param_get(param_precision, &precision);
+			}
+
+			orb_check(_command_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
+				handle_command(&_command);
+			}
+
+
+			float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
+			float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
+			ground_distance = _global_pos.alt - _target_position.alt;
+
+			// Distance to drop position and angle error to approach vector
+			// are relevant in all states greater than target valid (which calculates these positions)
+			if (_drop_state > DROP_STATE_TARGET_VALID) {
+				distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
+
+				float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
+				float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
+
+				approach_error = _wrap_pi(ground_direction - approach_direction);
+
+				if (counter % 90 == 0) {
+					mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
+				}
+			}
+
+			switch (_drop_state) {
+
+				case DROP_STATE_TARGET_VALID:
+				{
+
+					az = g;							// acceleration in z direction[m/s^2]
+					vz = 0; 						// velocity in z direction [m/s]
+					z = 0; 							// fallen distance [m]
+					h_0 = _global_pos.alt - _target_position.alt; 		// height over target at start[m]
+					h = h_0;						// height over target [m]
+					ax = 0;							// acceleration in x direction [m/s^2]
+					vx = groundspeed_body;// XXX project					// ground speed in x direction [m/s]
+					x = 0;							// traveled distance in x direction [m]
+					vw = 0;							// wind speed [m/s]
+					vrx = 0;						// relative velocity in x direction [m/s]
+					v = groundspeed_body;					// relative speed vector [m/s]
+					Fd = 0;							// Drag force [N]
+					Fdx = 0;						// Drag force in x direction [N]
+					Fdz = 0;						// Drag force in z direction [N]
+
+
+					// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
+					while (h > 0.05f) {
+						// z-direction
+						vz = vz + az * dt_freefall_prediction;
+						z = z + vz * dt_freefall_prediction;
+						h = h_0 - z;
+
+						// x-direction
+						vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
+						vx = vx + ax * dt_freefall_prediction;
+						x = x + vx * dt_freefall_prediction;
+						vrx = vx + vw;
+
+						// drag force
+						v = sqrtf(vz * vz + vrx * vrx);
+						Fd = 0.5f * rho * A * cd * (v * v);
+						Fdx = Fd * vrx / v;
+						Fdz = Fd * vz / v;
+
+						// acceleration
+						az = g - Fdz / m;
+						ax = -Fdx / m;
+					}
+
+					// compute drop vector
+					x = groundspeed_body * t_signal + x;
+
+					x_t = 0.0f;
+					y_t = 0.0f;
+
+					float wind_direction_n, wind_direction_e;
+
+					if (windspeed_norm < 0.5f) {	// If there is no wind, an arbitrarily direction is chosen
+						wind_direction_n = 1.0f;
+						wind_direction_e = 0.0f;
+
+					} else {
+						wind_direction_n = wind.windspeed_north / windspeed_norm;
+						wind_direction_e = wind.windspeed_east / windspeed_norm;
+					}
+
+					x_drop = x_t + x * wind_direction_n;
+					y_drop = y_t + x * wind_direction_e;
+					map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
+					_drop_position.alt = _target_position.alt + _alt_clearance;
+
+					// Compute flight vector
+					map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
+								 &(flight_vector_s.lat), &(flight_vector_s.lon));
+					flight_vector_s.altitude = _drop_position.alt;
+					map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
+								 &flight_vector_e.lat, &flight_vector_e.lon);
+					flight_vector_e.altitude = _drop_position.alt;
+
+					// Save WPs in datamanager
+					const ssize_t len = sizeof(struct mission_item_s);
+
+					if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
+						warnx("ERROR: could not save onboard WP");
+					}
+
+					if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
+						warnx("ERROR: could not save onboard WP");
+					}
+
+					_onboard_mission.count = 2;
+					_onboard_mission.current_seq = 0;
+
+					if (_onboard_mission_pub > 0) {
+						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+
+					} else {
+						_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
+					}
+
+					float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
+					mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));
+
+					_drop_state = DROP_STATE_TARGET_SET;
+				}
+				break;
+
+				case DROP_STATE_TARGET_SET:
+				{
+					float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
+
+					if (distance_wp2 < distance_real) {
+						_onboard_mission.current_seq = 0;
+						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+					} else {
+
+						// We're close enough - open the bay
+						distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));
+
+						if (isfinite(distance_real) && distance_real < distance_open_door &&
+							fabsf(approach_error) < math::radians(20.0f)) {
+							open_bay();
+							_drop_state = DROP_STATE_BAY_OPEN;
+							mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+						}
+					}
+				}
+				break;
+
+				case DROP_STATE_BAY_OPEN:
+					{
+						if (_drop_approval) {
+							map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
+							x_f = x_l + _global_pos.vel_n * dt_runs;
+							y_f = y_l + _global_pos.vel_e * dt_runs;
+							map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
+							future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
+
+							if (isfinite(distance_real) &&
+								(distance_real < precision) && ((distance_real < future_distance))) {
+									drop();
+									_drop_state = DROP_STATE_DROPPED;
+									mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
+							} else {
+
+								float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
+
+								if (distance_wp2 < distance_real) {
+									_onboard_mission.current_seq = 0;
+									orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+								}
+							}
+						}
+					}
+					break;
+
+				case DROP_STATE_DROPPED:
+					/* 2s after drop, reset and close everything again */
+					if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
+						_drop_state = DROP_STATE_INIT;
+						_drop_approval = false;
+						lock_release();
+						close_bay();
+						mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+
+						// remove onboard mission
+						_onboard_mission.current_seq = -1;
+						_onboard_mission.count = 0;
+						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+					}
+					break;
+			}
+
+			counter++;
+
+			// update_actuators();
+
+			// run at roughly 100 Hz
+			usleep(sleeptime_us);
+
+			dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
+			last_run = hrt_absolute_time();
+		}
+	}
+
+	warnx("exiting.");
+
+	_main_task = -1;
+	_exit(0);
+}
+
+void
+BottleDrop::handle_command(struct vehicle_command_s *cmd)
+{
+	switch (cmd->command) {
+	case VEHICLE_CMD_CUSTOM_0:
+		/*
+		 * param1 and param2 set to 1: open and drop
+		 * param1 set to 1: open
+		 * else: close (and don't drop)
+		 */
+		if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
+			open_bay();
+			drop();
+			mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+
+		} else if (cmd->param1 > 0.5f) {
+			open_bay();
+			mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+
+		} else {
+			lock_release();
+			close_bay();
+			mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+		}
+
+		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+		break;
+
+	case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+
+		switch ((int)(cmd->param1 + 0.5f)) {
+		case 0:
+			_drop_approval = false;
+			mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+			break;
+
+		case 1:
+			_drop_approval = true;
+			mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+			break;
+
+		default:
+			_drop_approval = false;
+			warnx("param1 val unknown");
+			break;
+		}
+
+		// XXX check all fields (2-3)
+		_alt_clearance = cmd->param4;
+		_target_position.lat = cmd->param5;
+		_target_position.lon = cmd->param6;
+		_target_position.alt = cmd->param7;
+		_drop_state = DROP_STATE_TARGET_VALID;
+		mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
+			(double)_target_position.lon, (double)_target_position.alt);
+		map_projection_init(&ref, _target_position.lat, _target_position.lon);
+		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+		break;
+
+	case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
+
+		if (cmd->param1 < 0) {
+
+			// Clear internal states
+			_drop_approval = false;
+			_drop_state = DROP_STATE_INIT;
+
+			// Abort if mission is present
+			_onboard_mission.current_seq = -1;
+
+			if (_onboard_mission_pub > 0) {
+				orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+			}
+
+		} else {
+			switch ((int)(cmd->param1 + 0.5f)) {
+			case 0:
+				_drop_approval = false;
+				break;
+
+			case 1:
+				_drop_approval = true;
+				mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
+				break;
+
+			default:
+				_drop_approval = false;
+				break;
+				// XXX handle other values
+			}
+		}
+
+		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+	case VEHICLE_CMD_RESULT_ACCEPTED:
+		break;
+
+	case VEHICLE_CMD_RESULT_DENIED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+		break;
+
+	case VEHICLE_CMD_RESULT_FAILED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+		break;
+
+	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+		break;
+
+	case VEHICLE_CMD_RESULT_UNSUPPORTED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+BottleDrop::task_main_trampoline(int argc, char *argv[])
+{
+	bottle_drop::g_bottle_drop->task_main();
+}
+
+static void usage()
+{
+	errx(1, "usage: bottle_drop {start|stop|status}");
+}
+
+int bottle_drop_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (bottle_drop::g_bottle_drop != nullptr) {
+			errx(1, "already running");
+		}
+
+		bottle_drop::g_bottle_drop = new BottleDrop;
+
+		if (bottle_drop::g_bottle_drop == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != bottle_drop::g_bottle_drop->start()) {
+			delete bottle_drop::g_bottle_drop;
+			bottle_drop::g_bottle_drop = nullptr;
+			err(1, "start failed");
+		}
+
+		return 0;
+	}
+
+	if (bottle_drop::g_bottle_drop == nullptr) {
+		errx(1, "not running");
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		delete bottle_drop::g_bottle_drop;
+		bottle_drop::g_bottle_drop = nullptr;
+
+	} else if (!strcmp(argv[1], "status")) {
+		bottle_drop::g_bottle_drop->status();
+
+	} else if (!strcmp(argv[1], "drop")) {
+		bottle_drop::g_bottle_drop->drop();
+
+	} else if (!strcmp(argv[1], "open")) {
+		bottle_drop::g_bottle_drop->open_bay();
+
+	} else if (!strcmp(argv[1], "close")) {
+		bottle_drop::g_bottle_drop->close_bay();
+
+	} else if (!strcmp(argv[1], "lock")) {
+		bottle_drop::g_bottle_drop->lock_release();
+
+	} else {
+		usage();
+	}
+
+	return 0;
+}
diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c
new file mode 100644
index 0000000000..51ebfb9a19
--- /dev/null
+++ b/src/modules/bottle_drop/bottle_drop_params.c
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bottle_drop_params.c
+ * Bottle drop parameters
+ *
+ * @author Dominik Juchli <juchlid@ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Ground drag property
+ *
+ * This parameter encodes the ground drag coefficient and the corresponding
+ * decrease in wind speed from the plane altitude to ground altitude.
+ *
+ * @unit unknown
+ * @min 0.001
+ * @max 0.1
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f);
+
+/**
+ * Plane turn radius
+ *
+ * The planes known minimal turn radius - use a higher value
+ * to make the plane maneuver more distant from the actual drop
+ * position. This is to ensure the wings are level during the drop.
+ *
+ * @unit meter
+ * @min 30.0
+ * @max 500.0
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f);
+
+/**
+ * Drop precision
+ *
+ * If the system is closer than this distance on passing over the
+ * drop position, it will release the payload. This is a safeguard
+ * to prevent a drop out of the required accuracy.
+ *
+ * @unit meter
+ * @min 1.0
+ * @max 80.0
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f);
+
+/**
+ * Payload drag coefficient of the dropped object
+ *
+ * The drag coefficient (cd) is the typical drag
+ * constant for air. It is in general object specific,
+ * but the closest primitive shape to the actual object
+ * should give good results:
+ * http://en.wikipedia.org/wiki/Drag_coefficient
+ *
+ * @unit meter
+ * @min 0.08
+ * @max 1.5
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f);
+
+/**
+ * Payload mass
+ *
+ * A typical small toy ball:
+ *   0.025 kg
+ *
+ * OBC water bottle:
+ *   0.6 kg
+ *
+ * @unit kilogram
+ * @min 0.001
+ * @max 5.0
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f);
+
+/**
+ * Payload front surface area
+ *
+ * A typical small toy ball:
+ *   (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2
+ *
+ * OBC water bottle:
+ *   (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2
+ *
+ * @unit m^2
+ * @min 0.001
+ * @max 0.5
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f);
diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk
new file mode 100644
index 0000000000..6b18fff556
--- /dev/null
+++ b/src/modules/bottle_drop/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Daemon application
+#
+
+MODULE_COMMAND		= bottle_drop
+
+SRCS			= bottle_drop.cpp \
+			  bottle_drop_params.c
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
new file mode 100644
index 0000000000..d4270b1530
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -0,0 +1,283 @@
+/*
+ * mc_att_control_base.cpp
+ *
+ *  Created on: Sep 25, 2014
+ *      Author: roman
+ */
+
+#include "mc_att_control_base.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+
+		_task_should_exit(false), _control_task(-1),
+
+		_actuators_0_circuit_breaker_enabled(false),
+
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
+
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+}
+
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
+}
+
+void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() {
+}
+
+void MulticopterAttitudeControlBase::control_attitude(float dt) {
+	float yaw_sp_move_rate = 0.0f;
+	bool publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled
+				|| _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.z;
+			publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			/* move yaw setpoint */
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(
+					_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < -yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x
+					* _params.man_pitch_max;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		R_sp.set(&_v_att_sp.R_body[0][0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body,
+				_v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
+				sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+//	/* publish the attitude setpoint if needed */
+//	if (publish_att_sp) {
+//		_v_att_sp.timestamp = hrt_absolute_time();
+//
+//		if (_att_sp_pub > 0) {
+//			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+//					&_v_att_sp);
+//
+//		} else {
+//			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+//					&_v_att_sp);
+//		}
+//	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = R
+				* (_I + e_R_cp * e_R_z_sin
+						+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(R.transposed() * R_sp);
+		math::Vector < 3 > e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
+			_params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
+	/* reset integral if disarmed */
+	if (!_armed.armed) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector < 3 > rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector < 3 > rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err)
+			+ _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i)
+						+ _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(
+						rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+						_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+
+}
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
new file mode 100644
index 0000000000..d75088b85b
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -0,0 +1,172 @@
+/*
+ * mc_att_control_base.h
+ *
+ *  Created on: Sep 25, 2014
+ *      Author: roman
+ */
+
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_main.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.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/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+//#include <systemlib/systemlib.h>
+#include <lib/mathlib/mathlib.h>
+
+
+/**
+ * Multicopter attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlBase {
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlBase();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~MulticopterAttitudeControlBase();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+
+protected:
+
+	bool _task_should_exit; /**< if true, sensor task should exit */
+	int _control_task; /**< task handle for sensor task */
+
+
+
+
+	bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
+
+	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
+	struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
+	struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
+	struct actuator_controls_s _actuators; /**< actuator controls */
+	struct actuator_armed_s _armed; /**< actuator arming status */
+
+	perf_counter_t _loop_perf; /**< loop performance counter */
+
+	math::Vector<3> _rates_prev; /**< angular rates on previous step */
+	math::Vector<3> _rates_sp; /**< angular rates setpoint */
+	math::Vector<3> _rates_int; /**< angular rates integral error */
+	float _thrust_sp; /**< thrust setpoint */
+	math::Vector<3> _att_control; /**< attitude control vector */
+
+	math::Matrix<3, 3> _I; /**< identity matrix */
+
+	bool _reset_yaw_sp; /**< reset yaw setpoint flag */
+
+
+
+	struct {
+		math::Vector<3> att_p; /**< P gain for angular error */
+		math::Vector<3> rate_p; /**< P gain for angular rate error */
+		math::Vector<3> rate_i; /**< I gain for angular rate error */
+		math::Vector<3> rate_d; /**< D gain for angular rate error */
+		float yaw_ff; /**< yaw control feed-forward */
+		float yaw_rate_max; /**< max yaw rate */
+
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+	} _params;
+
+
+	/**
+	 * Attitude controller.
+	 */
+	//void control_attitude(float dt);
+
+	/**
+	 * Attitude rates controller.
+	 */
+	void control_attitude(float dt);
+	void control_attitude_rates(float dt);
+
+	void vehicle_attitude_setpoint_poll();	//provisional
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
new file mode 100644
index 0000000000..66f1c8c731
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file datalinkloss.cpp
+ * Helper class for Data Link Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA	0.01f
+
+DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_commsholdwaittime(this, "CH_T"),
+	_param_commsholdlat(this, "CH_LAT"),
+	_param_commsholdlon(this, "CH_LON"),
+	_param_commsholdalt(this, "CH_ALT"),
+	_param_airfieldhomelat(this, "NAV_AH_LAT", false),
+	_param_airfieldhomelon(this, "NAV_AH_LON", false),
+	_param_airfieldhomealt(this, "NAV_AH_ALT", false),
+	_param_airfieldhomewaittime(this, "AH_T"),
+	_param_numberdatalinklosses(this, "N"),
+	_param_skipcommshold(this, "CHSK"),
+	_dll_state(DLL_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+DataLinkLoss::~DataLinkLoss()
+{
+}
+
+void
+DataLinkLoss::on_inactive()
+{
+	/* reset DLL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_dll_state = DLL_STATE_NONE;
+	}
+}
+
+void
+DataLinkLoss::on_activation()
+{
+	_dll_state = DLL_STATE_NONE;
+	updateParams();
+	advance_dll();
+	set_dll_item();
+}
+
+void
+DataLinkLoss::on_active()
+{
+	if (is_mission_item_reached()) {
+		updateParams();
+		advance_dll();
+		set_dll_item();
+	}
+}
+
+void
+DataLinkLoss::set_dll_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_dll_state) {
+	case DLL_STATE_FLYTOCOMMSHOLDWP: {
+		_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _param_commsholdalt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
+		_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _param_airfieldhomealt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case DLL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("not switched to manual: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+DataLinkLoss::advance_dll()
+{
+	switch (_dll_state) {
+	case DLL_STATE_NONE:
+		/* Check the number of data link losses. If above home fly home directly */
+		/* if number of data link losses limit is not reached fly to comms hold wp */
+		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			warnx("%d data link losses, limit is %d, fly to airfield home",
+					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		} else {
+			if (!_param_skipcommshold.get()) {
+				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
+				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			} else {
+				/* comms hold wp not active, fly to airfield home directly */
+				warnx("Skipping comms hold wp. Flying directly to airfield home");
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+			}
+		}
+		break;
+	case DLL_STATE_FLYTOCOMMSHOLDWP:
+		warnx("fly to airfield home");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
+		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
+		_dll_state = DLL_STATE_TERMINATE;
+		warnx("time is up, state should have been changed manually by now");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_TERMINATE:
+		warnx("dll end");
+		_dll_state = DLL_STATE_END;
+		break;
+
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
new file mode 100644
index 0000000000..31e0e39078
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.h
@@ -0,0 +1,98 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file datalinkloss.h
+ * Helper class for Data Link Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_DATALINKLOSS_H
+#define NAVIGATOR_DATALINKLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class DataLinkLoss : public MissionBlock
+{
+public:
+	DataLinkLoss(Navigator *navigator, const char *name);
+
+	~DataLinkLoss();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_commsholdwaittime;
+	control::BlockParamInt _param_commsholdlat; // * 1e7
+	control::BlockParamInt _param_commsholdlon; // * 1e7
+	control::BlockParamFloat _param_commsholdalt;
+	control::BlockParamInt _param_airfieldhomelat; // * 1e7
+	control::BlockParamInt _param_airfieldhomelon; // * 1e7
+	control::BlockParamFloat _param_airfieldhomealt;
+	control::BlockParamFloat _param_airfieldhomewaittime;
+	control::BlockParamInt _param_numberdatalinklosses;
+	control::BlockParamInt _param_skipcommshold;
+
+	enum DLLState {
+		DLL_STATE_NONE = 0,
+		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
+		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+		DLL_STATE_TERMINATE = 3,
+		DLL_STATE_END = 4
+	} _dll_state;
+
+	/**
+	 * Set the DLL item
+	 */
+	void		set_dll_item();
+
+	/**
+	 * Move to next DLL item
+	 */
+	void		advance_dll();
+
+};
+#endif
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
new file mode 100644
index 0000000000..6780c0c88c
--- /dev/null
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file datalinkloss_params.c
+ *
+ * Parameters for DLL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Data Link Loss parameters, accessible via MAVLink
+ */
+
+/**
+ * Comms hold wait time
+ *
+ * The amount of time in seconds the system should wait at the comms hold waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
+
+/**
+ * Comms hold Lat
+ *
+ * Latitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
+
+/**
+ * Comms hold Lon
+ *
+ * Longitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
+
+/**
+ * Comms hold alt
+ *
+ * Altitude of comms hold waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
+
+/**
+ * Aifield hole wait time
+ *
+ * The amount of time in seconds the system should wait at the airfield home waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
+
+/**
+ * Number of allowed Datalink timeouts
+ *
+ * After more than this number of data link timeouts the aircraft returns home directly
+ *
+ * @group DLL
+ * @min 0
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(NAV_DLL_N, 2);
+
+/**
+ * Skip comms hold wp
+ *
+ * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
+ * airfield home
+ *
+ * @group DLL
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);
diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp
new file mode 100644
index 0000000000..e007b208b2
--- /dev/null
+++ b/src/modules/navigator/enginefailure.cpp
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /* @file enginefailure.cpp
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+
+#include "navigator.h"
+#include "enginefailure.h"
+
+#define DELAY_SIGMA	0.01f
+
+EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_ef_state(EF_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+EngineFailure::~EngineFailure()
+{
+}
+
+void
+EngineFailure::on_inactive()
+{
+	_ef_state = EF_STATE_NONE;
+}
+
+void
+EngineFailure::on_activation()
+{
+	_ef_state = EF_STATE_NONE;
+	advance_ef();
+	set_ef_item();
+}
+
+void
+EngineFailure::on_active()
+{
+	if (is_mission_item_reached()) {
+		advance_ef();
+		set_ef_item();
+	}
+}
+
+void
+EngineFailure::set_ef_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* make sure we have the latest params */
+	updateParams();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_ef_state) {
+	case EF_STATE_LOITERDOWN: {
+		//XXX create mission item at ground (below?) here
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		 //XXX setting altitude to a very low value, evaluate other options
+		_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+EngineFailure::advance_ef()
+{
+	switch (_ef_state) {
+	case EF_STATE_NONE:
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
+		_ef_state = EF_STATE_LOITERDOWN;
+		break;
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h
new file mode 100644
index 0000000000..2c48c2fce2
--- /dev/null
+++ b/src/modules/navigator/enginefailure.h
@@ -0,0 +1,83 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file enginefailure.h
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_ENGINEFAILURE_H
+#define NAVIGATOR_ENGINEFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class EngineFailure : public MissionBlock
+{
+public:
+	EngineFailure(Navigator *navigator, const char *name);
+
+	~EngineFailure();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	enum EFState {
+		EF_STATE_NONE = 0,
+		EF_STATE_LOITERDOWN = 1,
+	} _ef_state;
+
+	/**
+	 * Set the DLL item
+	 */
+	void		set_ef_item();
+
+	/**
+	 * Move to next EF item
+	 */
+	void		advance_ef();
+
+};
+#endif
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
new file mode 100644
index 0000000000..cd55f60b06
--- /dev/null
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file gpsfailure.cpp
+ * Helper class for gpsfailure mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "gpsfailure.h"
+
+#define DELAY_SIGMA	0.01f
+
+GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_loitertime(this, "LT"),
+	_param_openlooploiter_roll(this, "R"),
+	_param_openlooploiter_pitch(this, "P"),
+	_param_openlooploiter_thrust(this, "TR"),
+	_gpsf_state(GPSF_STATE_NONE),
+	_timestamp_activation(0)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+GpsFailure::~GpsFailure()
+{
+}
+
+void
+GpsFailure::on_inactive()
+{
+	/* reset GPSF state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_gpsf_state = GPSF_STATE_NONE;
+	}
+}
+
+void
+GpsFailure::on_activation()
+{
+	_gpsf_state = GPSF_STATE_NONE;
+	_timestamp_activation = hrt_absolute_time();
+	updateParams();
+	advance_gpsf();
+	set_gpsf_item();
+}
+
+void
+GpsFailure::on_active()
+{
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_LOITER: {
+		/* Position controller does not run in this mode:
+		 * navigator has to publish an attitude setpoint */
+		_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
+		_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
+		_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
+		_navigator->publish_att_sp();
+
+		/* Measure time */
+		hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
+
+		//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
+				//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
+		if (elapsed > _param_loitertime.get() * 1e6f) {
+			/* no recovery, adavance the state machine */
+			warnx("gps not recovered, switch to next state");
+			advance_gpsf();
+		}
+		break;
+	}
+	case GPSF_STATE_TERMINATE:
+		set_gpsf_item();
+		advance_gpsf();
+		break;
+	default:
+		break;
+	}
+}
+
+void
+GpsFailure::set_gpsf_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* Set pos sp triplet to invalid to stop pos controller */
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = false;
+	pos_sp_triplet->next.valid = false;
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("gps fail: request flight termination");
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+GpsFailure::advance_gpsf()
+{
+	updateParams();
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_NONE:
+		_gpsf_state = GPSF_STATE_LOITER;
+		warnx("gpsf loiter");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
+		break;
+	case GPSF_STATE_LOITER:
+		_gpsf_state = GPSF_STATE_TERMINATE;
+		warnx("gpsf terminate");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
+		warnx("mavlink sent");
+		break;
+	case GPSF_STATE_TERMINATE:
+		warnx("gpsf end");
+		_gpsf_state = GPSF_STATE_END;
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h
new file mode 100644
index 0000000000..e9e72babf8
--- /dev/null
+++ b/src/modules/navigator/gpsfailure.h
@@ -0,0 +1,97 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file gpsfailure.h
+ * Helper class for Data Link Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_GPSFAILURE_H
+#define NAVIGATOR_GPSFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class GpsFailure : public MissionBlock
+{
+public:
+	GpsFailure(Navigator *navigator, const char *name);
+
+	~GpsFailure();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_loitertime;
+	control::BlockParamFloat _param_openlooploiter_roll;
+	control::BlockParamFloat _param_openlooploiter_pitch;
+	control::BlockParamFloat _param_openlooploiter_thrust;
+
+	enum GPSFState {
+		GPSF_STATE_NONE = 0,
+		GPSF_STATE_LOITER = 1,
+		GPSF_STATE_TERMINATE = 2,
+		GPSF_STATE_END = 3,
+	} _gpsf_state;
+
+	hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+
+	/**
+	 * Set the GPSF item
+	 */
+	void		set_gpsf_item();
+
+	/**
+	 * Move to next GPSF item
+	 */
+	void		advance_gpsf();
+
+};
+#endif
diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c
new file mode 100644
index 0000000000..39d179eed7
--- /dev/null
+++ b/src/modules/navigator/gpsfailure_params.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gpsfailure_params.c
+ *
+ * Parameters for GPSF navigation mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * GPS Failure Navigation Mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter time
+ *
+ * The amount of time in seconds the system should do open loop loiter and wait for gps recovery
+ * before it goes into flight termination.
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+
+/**
+ * Open loop loiter roll
+ *
+ * Roll in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
+
+/**
+ * Open loop loiter pitch
+ *
+ * Pitch in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min -30.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
+
+/**
+ * Open loop loiter thrust
+ *
+ * Thrust value which is set during the open loop loiter
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+
+
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
new file mode 100644
index 0000000000..5564a1c42e
--- /dev/null
+++ b/src/modules/navigator/rcloss.cpp
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file rcloss.cpp
+ * Helper class for RC Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA	0.01f
+
+RCLoss::RCLoss(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_loitertime(this, "LT"),
+	_rcl_state(RCL_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+RCLoss::~RCLoss()
+{
+}
+
+void
+RCLoss::on_inactive()
+{
+	/* reset RCL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_rcl_state = RCL_STATE_NONE;
+	}
+}
+
+void
+RCLoss::on_activation()
+{
+	_rcl_state = RCL_STATE_NONE;
+	updateParams();
+	advance_rcl();
+	set_rcl_item();
+}
+
+void
+RCLoss::on_active()
+{
+	if (is_mission_item_reached()) {
+		updateParams();
+		advance_rcl();
+		set_rcl_item();
+	}
+}
+
+void
+RCLoss::set_rcl_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_rcl_state) {
+	case RCL_STATE_LOITER: {
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude = _navigator->get_global_position()->alt;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case RCL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("rc not recovered: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+RCLoss::advance_rcl()
+{
+	switch (_rcl_state) {
+	case RCL_STATE_NONE:
+		if (_param_loitertime.get() > 0.0f) {
+			warnx("RC loss, OBC mode, loiter");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering");
+			_rcl_state = RCL_STATE_LOITER;
+		} else {
+			warnx("RC loss, OBC mode, slip loiter, terminate");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
+			_rcl_state = RCL_STATE_TERMINATE;
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+		}
+		break;
+	case RCL_STATE_LOITER:
+		_rcl_state = RCL_STATE_TERMINATE;
+		warnx("time is up, no RC regain, terminating");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case RCL_STATE_TERMINATE:
+		warnx("rcl end");
+		_rcl_state = RCL_STATE_END;
+		break;
+	default:
+		break;
+	}
+}
diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h
new file mode 100644
index 0000000000..bcb74d8778
--- /dev/null
+++ b/src/modules/navigator/rcloss.h
@@ -0,0 +1,88 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file rcloss.h
+ * Helper class for RC Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_RCLOSS_H
+#define NAVIGATOR_RCLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class RCLoss : public MissionBlock
+{
+public:
+	RCLoss(Navigator *navigator, const char *name);
+
+	~RCLoss();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_loitertime;
+
+	enum RCLState {
+		RCL_STATE_NONE = 0,
+		RCL_STATE_LOITER = 1,
+		RCL_STATE_TERMINATE = 2,
+		RCL_STATE_END = 3
+	} _rcl_state;
+
+	/**
+	 * Set the RCL item
+	 */
+	void		set_rcl_item();
+
+	/**
+	 * Move to next RCL item
+	 */
+	void		advance_rcl();
+
+};
+#endif
diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c
new file mode 100644
index 0000000000..f1a01c73b5
--- /dev/null
+++ b/src/modules/navigator/rcloss_params.c
@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rcloss_params.c
+ *
+ * Parameters for RC Loss (OBC)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * OBC RC Loss mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter Time
+ *
+ * The amount of time in seconds the system should loiter at current position before termination
+ * Set to -1 to make the system skip loitering
+ *
+ * @unit seconds
+ * @min -1.0
+ * @group RCL
+ */
+PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);
diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h
new file mode 100644
index 0000000000..44c51e4d98
--- /dev/null
+++ b/src/modules/uORB/topics/multirotor_motor_limits.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file multirotor_motor_limits.h
+ *
+ * Definition of multirotor_motor_limits  topic
+ */
+
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+        uint8_t roll_pitch	: 1; // roll/pitch limit reached
+        uint8_t yaw		: 1; // yaw limit reached
+        uint8_t throttle_lower	: 1; // lower throttle limit reached
+        uint8_t throttle_upper	: 1; // upper throttle limit reached
+        uint8_t reserved	: 4;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
+
+#endif
diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h
new file mode 100644
index 0000000000..24ecca9faa
--- /dev/null
+++ b/src/modules/uORB/topics/vtol_vehicle_status.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vtol_status.h
+ *
+ * Vtol status topic
+ *
+ */
+
+#ifndef TOPIC_VTOL_STATUS_H
+#define TOPIC_VTOL_STATUS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/* Indicates in which mode the vtol aircraft is in */
+struct vtol_vehicle_status_s {
+
+	uint64_t	timestamp;	/**< Microseconds since system boot */
+	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vtol_vehicle_status);
+
+#endif
diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk
new file mode 100644
index 0000000000..c349c23401
--- /dev/null
+++ b/src/modules/vtol_att_control/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# VTOL attitude controller
+#
+
+MODULE_COMMAND	= vtol_att_control
+
+SRCS		= vtol_att_control_main.cpp
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
new file mode 100644
index 0000000000..38fa4eec18
--- /dev/null
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -0,0 +1,642 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VTOL_att_control_main.cpp
+ * Implementation of an attitude controller for VTOL airframes. This module receives data
+ * from both the fixed wing- and the multicopter attitude controllers and processes it.
+ * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
+ * flight or transition). It also publishes the resulting controls on the actuator controls topics.
+ *
+ * @author Roman Bapst 		<bapstr@ethz.ch>
+ * @author Lorenz Meier 	<lm@inf.ethz.ch>
+ * @author Thomas Gubler	<thomasgubler@gmail.com>
+ *
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.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/vehicle_control_mode.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include "drivers/drv_pwm_output.h"
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <nuttx/mtd.h>
+
+#include <fcntl.h>
+
+
+extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
+
+class VtolAttitudeControl
+{
+public:
+
+	VtolAttitudeControl();
+	~VtolAttitudeControl();
+
+	int start();	/* start the task and return OK on success */
+
+
+private:
+//******************flags & handlers******************************************************
+	bool _task_should_exit;
+	int _control_task;		//task handle for VTOL attitude controller
+
+	/* handlers for subscriptions */
+	int		_v_att_sub;				//vehicle attitude subscription
+	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
+	int		_v_rates_sp_sub;		//vehicle rates setpoint subscription
+	int		_v_control_mode_sub;	//vehicle control mode subscription
+	int		_params_sub;			//parameter updates subscription
+	int		_manual_control_sp_sub;	//manual control setpoint subscription
+	int		_armed_sub;				//arming status subscription
+
+	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
+	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
+
+	//handlers for publishers
+	orb_advert_t	_actuators_0_pub;		//input for the mixer (roll,pitch,yaw,thrust)
+	orb_advert_t 	_actuators_1_pub;
+	orb_advert_t	_vtol_vehicle_status_pub;
+//*******************data containers***********************************************************
+	struct vehicle_attitude_s			_v_att;				//vehicle attitude
+	struct vehicle_attitude_setpoint_s	_v_att_sp;			//vehicle attitude setpoint
+	struct vehicle_rates_setpoint_s		_v_rates_sp;		//vehicle rates setpoint
+	struct manual_control_setpoint_s	_manual_control_sp; //manual control setpoint
+	struct vehicle_control_mode_s		_v_control_mode;	//vehicle control mode
+	struct vtol_vehicle_status_s 		_vtol_vehicle_status;
+	struct actuator_controls_s			_actuators_out_0;	//actuator controls going to the mc mixer
+	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
+	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
+	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
+	struct actuator_armed_s				_armed;				//actuator arming status
+
+	struct {
+		float min_pwm_mc;	//pwm value for idle in mc mode
+	} _params;
+
+	struct {
+		param_t min_pwm_mc;
+	} _params_handles;
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	/* for multicopters it is usual to have a non-zero idle speed of the engines
+	 * for fixed wings we want to have an idle speed of zero since we do not want
+	 * to waste energy when gliding. */
+	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
+
+//*****************Member functions***********************************************************************
+
+	void 		task_main();	//main task
+	static void	task_main_trampoline(int argc, char *argv[]);	//Shim for calling task_main from task_create.
+
+	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+	void		vehicle_manual_poll();			//Check for changes in manual inputs.
+	void		arming_status_poll();			//Check for arming status updates.
+	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+	void 		parameters_update_poll();		//Check if parameters have changed
+	int 		parameters_update();			//Update local paraemter cache
+	void  		fill_mc_att_control_output();	//write mc_att_control results to actuator message
+	void		fill_fw_att_control_output();	//write fw_att_control results to actuator message
+	void 		set_idle_fw();
+	void 		set_idle_mc();
+};
+
+namespace VTOL_att_control
+{
+VtolAttitudeControl *g_control;
+}
+
+/**
+* Constructor
+*/
+VtolAttitudeControl::VtolAttitudeControl() :
+	_task_should_exit(false),
+	_control_task(-1),
+
+	//init subscription handlers
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+
+	//init publication handlers
+	_actuators_0_pub(-1),
+	_actuators_1_pub(-1),
+	_vtol_vehicle_status_pub(-1),
+
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+{
+
+	flag_idle_mc = true;	/*assume we always start in mc mode for a VTOL airframe */
+
+	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
+	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
+	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
+	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params_handles.min_pwm_mc = param_find("PWM_MIN");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+/**
+* Destructor
+*/
+VtolAttitudeControl::~VtolAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	VTOL_att_control::g_control = nullptr;
+}
+
+/**
+* Check for changes in vehicle control mode.
+*/
+void VtolAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check if vehicle control mode has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+/**
+* Check for changes in manual inputs.
+*/
+void VtolAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+/**
+* Check for arming status updates.
+*/
+void VtolAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+/**
+* Check for inputs from mc attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_mc_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_mc, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in);
+	}
+}
+
+/**
+* Check for inputs from fw attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_fw_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_fw, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in);
+	}
+}
+
+/**
+* Check for parameter updates.
+*/
+void
+VtolAttitudeControl::parameters_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+/**
+* Update parameters.
+*/
+int
+VtolAttitudeControl::parameters_update()
+{
+	/* idle pwm */
+	float v;
+	param_get(_params_handles.min_pwm_mc, &v);
+	_params.min_pwm_mc = v;
+
+	return OK;
+}
+
+/**
+* Prepare message to acutators with data from mc attitude controller.
+*/
+void VtolAttitudeControl::fill_mc_att_control_output()
+{
+	_actuators_out_0.control[0] = _actuators_mc_in.control[0];
+	_actuators_out_0.control[1] = _actuators_mc_in.control[1];
+	_actuators_out_0.control[2] = _actuators_mc_in.control[2];
+	_actuators_out_0.control[3] = _actuators_mc_in.control[3];
+	//set neutral position for elevons
+	_actuators_out_1.control[0] = 0;	//roll elevon
+	_actuators_out_1.control[1] = 0;	//pitch elevon
+}
+
+/**
+* Prepare message to acutators with data from fw attitude controller.
+*/
+void VtolAttitudeControl::fill_fw_att_control_output()
+{
+	/*For the first test in fw mode, only use engines for thrust!!!*/
+	_actuators_out_0.control[0] = 0;
+	_actuators_out_0.control[1] = 0;
+	_actuators_out_0.control[2] = 0;
+	_actuators_out_0.control[3] = _actuators_fw_in.control[3];
+	/*controls for the elevons */
+	_actuators_out_1.control[0] = _actuators_fw_in.control[0];	/*roll elevon*/
+	_actuators_out_1.control[1] = _actuators_fw_in.control[1];	/*pitch elevon */
+}
+
+/**
+* Adjust idle speed for fw mode.
+*/
+void VtolAttitudeControl::set_idle_fw()
+{
+	int ret;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	unsigned pwm_value = PWM_LOWEST_MIN;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < 4; i++) {
+
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+/**
+* Adjust idle speed for mc mode.
+*/
+void VtolAttitudeControl::set_idle_mc()
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	unsigned pwm_value = 1100;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < 4; i++) {
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+void
+VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	VTOL_att_control::g_control->task_main();
+}
+
+void VtolAttitudeControl::task_main()
+{
+	warnx("started");
+	fflush(stdout);
+
+	/* do subscriptions */
+	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_v_rates_sp_sub        = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
+
+	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
+	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
+
+	parameters_update();  /*initialize parameter cache/*
+
+	/* wakeup source*/
+	struct pollfd fds[3];	/*input_mc, input_fw, parameters*/
+
+	fds[0].fd     = _actuator_inputs_mc;
+	fds[0].events = POLLIN;
+	fds[1].fd     = _actuator_inputs_fw;
+	fds[1].events = POLLIN;
+	fds[2].fd     = _params_sub;
+	fds[2].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/*Advertise/Publish vtol vehicle status*/
+		if (_vtol_vehicle_status_pub > 0) {
+			orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
+
+		} else {
+			_vtol_vehicle_status.timestamp = hrt_absolute_time();
+			_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
+		}
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[2].revents & POLLIN) {	//parameters were updated, read them now
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+		vehicle_manual_poll();			//Check for changes in manual inputs.
+		arming_status_poll();			//Check for arming status updates.
+		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+		parameters_update_poll();
+
+		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = true;
+
+			if (!flag_idle_mc) {	/* we want to adjust idle speed for mc mode */
+				set_idle_mc();
+				flag_idle_mc = true;
+			}
+
+			/* got data from mc_att_controller */
+			if (fds[0].revents & POLLIN) {
+				vehicle_manual_poll();	/* update remote input */
+				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
+
+				fill_mc_att_control_output();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+
+		if (_manual_control_sp.aux1 >= 0.0f) {			/* vehicle is in fw mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = false;
+
+			if (flag_idle_mc) {	/* we want to adjust idle speed for fixed wing mode */
+				set_idle_fw();
+				flag_idle_mc = false;
+			}
+
+			if (fds[1].revents & POLLIN) {		/* got data from fw_att_controller */
+				orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
+				vehicle_manual_poll();	//update remote input
+
+				fill_fw_att_control_output();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+	}
+
+	warnx("exit");
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+VtolAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("vtol_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 10,
+				       2048,
+				       (main_t)&VtolAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+
+int vtol_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: vtol_att_control {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (VTOL_att_control::g_control != nullptr) {
+			errx(1, "already running");
+		}
+
+		VTOL_att_control::g_control = new VtolAttitudeControl;
+
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != VTOL_att_control::g_control->start()) {
+			delete VTOL_att_control::g_control;
+			VTOL_att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete VTOL_att_control::g_control;
+		VTOL_att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (VTOL_att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

From b168ba92e0be91aec9db5aaccbcc427cfd067c7b Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 29 Oct 2014 15:57:45 +0100
Subject: [PATCH 028/416] added setter functions into base class. used when
 integrating the base class into a gazebo environment

---
 .../mc_att_control/mc_att_control_base.cpp    | 57 +++++++++++++++++++
 .../mc_att_control/mc_att_control_base.h      |  7 +++
 2 files changed, 64 insertions(+)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index d4270b1530..d39bae4dc9 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -281,3 +281,60 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 	}
 
 }
+
+void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double>& attitude) {
+	// check if this is consistent !!!
+	math::Vector<3> quat;
+	quat(0) = attitude(0);
+	quat(1) = attitude(1);
+	quat(2) = attitude(2);
+	quat(3) = attitude(3);
+
+	_v_att.q[0] = quat(0);
+	_v_att.q[1] = quat(1);
+	_v_att.q[2] = quat(2);
+	_v_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0,0);
+	_v_att.R[1][0] = Rot(1,0);
+	_v_att.R[2][0] = Rot(2,0);
+	_v_att.R[0][1] = Rot(0,1);
+	_v_att.R[1][1] = Rot(1,1);
+	_v_att.R[2][1] = Rot(2,1);
+	_v_att.R[0][2] = Rot(0,2);
+	_v_att.R[1][2] = Rot(1,2);
+	_v_att.R[2][2] = Rot(2,2);
+
+	_v_att.R_valid = true;
+
+}
+
+void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	// check if this is consistent !!!
+	_v_att.rollspeed  = angular_rate(0);
+	_v_att.pitchspeed = angular_rate(1);
+	_v_att.yawspeed   = angular_rate(2);
+
+}
+
+void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_v_att_sp.thrust     = control_attitude_thrust_reference(3);
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
+
+}
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index d75088b85b..995838bb27 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -166,6 +166,13 @@ protected:
 
 	void vehicle_attitude_setpoint_poll();	//provisional
 
+	// setters and getters for interface with euroc-gazebo simulator
+	void set_attitude(const Eigen::Quaternion<double>& attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	
+
+
 
 };
 

From 6ecefe7930d851a033b4fa414366dec223560a50 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 4 Nov 2014 22:52:30 +0100
Subject: [PATCH 029/416] Fix include guards for ROS, needs cleanups

---
 include/px4.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/px4.h b/include/px4.h
index 7dedd5f822..abc10276f1 100644
--- a/include/px4.h
+++ b/include/px4.h
@@ -37,11 +37,11 @@
  * Main system header with common convenience functions
  */
 
-#ifdef __linux
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 #include "ros/ros.h"
 #define px4_warnx ROS_WARN
 #define px4_infox ROS_INFO
 #else
 #define px4_warnx warnx
 #define px4_infox warnx
-#endif
\ No newline at end of file
+#endif

From c51ec3411850d9de0794d4e584838d4a137dafcf Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 5 Nov 2014 08:50:20 +0100
Subject: [PATCH 030/416] added initialisation of parameters, added assignment
 of actuator controls, cleaned up

---
 .../mc_att_control/mc_att_control_base.cpp    | 53 ++++++++++++++++---
 1 file changed, 45 insertions(+), 8 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index d39bae4dc9..fee1174582 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -51,6 +51,27 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_att_control.zero();
 
 	_I.identity();
+
+	// setup standard gains
+	_params.att_p(0)      = 5.0;
+	_params.rate_p(0)     = 0.05;
+	_params.rate_i(0)     = 0.0;
+	_params.rate_d(0)     = 0.003;
+	/* pitch gains */
+	_params.att_p(1)      = 5.0;
+	_params.rate_p(1)     = 0.05;
+	_params.rate_i(1)     = 0.0;
+	_params.rate_d(1)     = 0.003;
+	/* yaw gains */
+	_params.att_p(2)      = 2.8;
+	_params.rate_p(2)     = 0.2;
+	_params.rate_i(2)     = 0.1;
+	_params.rate_d(2)     = 0.0;
+	_params.yaw_rate_max  = 0.5;
+	_params.yaw_ff        = 0.5;
+	_params.man_roll_max  = 0.6;
+	_params.man_pitch_max = 0.6;
+	_params.man_yaw_max   = 0.6;
 }
 
 MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
@@ -282,13 +303,21 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 
 }
 
-void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double>& attitude) {
+void MulticopterAttitudeControlBase::set_actuator_controls() {
+	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+	//_actuators.timestamp = hrt_absolute_time();
+}
+
+void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
 	// check if this is consistent !!!
-	math::Vector<3> quat;
-	quat(0) = attitude(0);
-	quat(1) = attitude(1);
-	quat(2) = attitude(2);
-	quat(3) = attitude(3);
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
 
 	_v_att.q[0] = quat(0);
 	_v_att.q[1] = quat(1);
@@ -322,11 +351,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4
 	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
 	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
 	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_v_att_sp.thrust     = control_attitude_thrust_reference(3);
+	_v_att_sp.thrust     = (control_attitude_thrust_referenc(3) -30)*(-1)/30;
 
 	// setup rotation matrix
 	math::Matrix<3,3> Rot_sp;
-	Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
+	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
 	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
 	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
 	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
@@ -338,3 +367,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4
 	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
 
 }
+
+void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}
+

From 48cdbf3d0cb8aed7dcdddde872a64096e7c9a595 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 5 Nov 2014 08:51:12 +0100
Subject: [PATCH 031/416] added more setter and getter functions

---
 .../mc_att_control/mc_att_control_base.h      | 19 ++++++++++++-------
 1 file changed, 12 insertions(+), 7 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 995838bb27..8f1cf015ea 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -76,6 +76,8 @@
 #include <systemlib/perf_counter.h>
 //#include <systemlib/systemlib.h>
 #include <lib/mathlib/mathlib.h>
+#include <Eigen/Eigen>
+
 
 
 /**
@@ -105,6 +107,14 @@ public:
 	 *
 	 * @return		OK on success.
 	 */
+	void control_attitude(float dt);
+	void control_attitude_rates(float dt);
+	 // setters and getters for interface with euroc-gazebo simulator
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	void set_actuator_controls();
 
 protected:
 
@@ -161,15 +171,10 @@ protected:
 	/**
 	 * Attitude rates controller.
 	 */
-	void control_attitude(float dt);
-	void control_attitude_rates(float dt);
-
+	
 	void vehicle_attitude_setpoint_poll();	//provisional
 
-	// setters and getters for interface with euroc-gazebo simulator
-	void set_attitude(const Eigen::Quaternion<double>& attitude);
-	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
-	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	
 	
 
 

From 1e9f431cf1d93b64f26fb9c3e42329e7237bad0e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 5 Nov 2014 15:03:00 +0100
Subject: [PATCH 032/416] ros example: send rc_channels message

---
 CMakeLists.txt                         |  1 +
 include/px4.h                          |  6 ++++++
 package.xml                            |  8 ++++----
 src/examples/publisher/publisher.cpp   | 16 ++++++++--------
 src/examples/subscriber/subscriber.cpp |  9 +++++----
 5 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 144bdcbc34..de2907e6ab 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -84,6 +84,7 @@ catkin_package(
 #  LIBRARIES px4
 #  CATKIN_DEPENDS roscpp rospy std_msgs
 #  DEPENDS system_lib
+  CATKIN_DEPENDS message_runtime
 )
 
 ###########
diff --git a/include/px4.h b/include/px4.h
index abc10276f1..4684cd256f 100644
--- a/include/px4.h
+++ b/include/px4.h
@@ -38,10 +38,16 @@
  */
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/*
+ * Building for running within the ROS environment
+ */
 #include "ros/ros.h"
 #define px4_warnx ROS_WARN
 #define px4_infox ROS_INFO
 #else
+/*
+ * Building for NuttX
+ */
 #define px4_warnx warnx
 #define px4_infox warnx
 #endif
diff --git a/package.xml b/package.xml
index 3575119be7..bc23cdd18f 100644
--- a/package.xml
+++ b/package.xml
@@ -4,7 +4,7 @@
   <version>1.0.0</version>
   <description>The PX4 Flight Stack package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="lorenz@px4.io">Lorenz Meier</maintainer>
@@ -32,11 +32,11 @@
   <!-- Dependencies can be catkin packages or system dependencies -->
   <!-- Examples: -->
   <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
+  <build_depend>message_generation</build_depend>
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
+  <run_depend>message_runtime</run_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
@@ -56,4 +56,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
\ No newline at end of file
+</package>
diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index cc88cd543d..53fe2e9051 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -33,6 +33,7 @@
 // %EndTag(MSG_HEADER)%
 
 #include <sstream>
+#include <px4/rc_channels.h>
 
 /**
  * This tutorial demonstrates simple sending of messages over the ROS system.
@@ -80,7 +81,7 @@ int main(int argc, char **argv)
    * buffer up before throwing some away.
    */
 // %Tag(PUBLISHER)%
-  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
+  ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
 // %EndTag(PUBLISHER)%
 
 // %Tag(LOOP_RATE)%
@@ -100,15 +101,14 @@ int main(int argc, char **argv)
      * This is a message object. You stuff it with data, and then publish it.
      */
 // %Tag(FILL_MESSAGE)%
-    std_msgs::String msg;
+    px4::rc_channels msg;
 
-    std::stringstream ss;
-    ss << "hello world " << count;
-    msg.data = ss.str();
+    ros::Time time = ros::Time::now();
+    msg.timestamp_last_valid = time.sec * 1e6 + time.nsec;
 // %EndTag(FILL_MESSAGE)%
 
 // %Tag(ROSCONSOLE)%
-    px4_warnx("%s", msg.data.c_str());
+   px4_warnx("%lu", msg.timestamp_last_valid);
 // %EndTag(ROSCONSOLE)%
 
     /**
@@ -118,7 +118,7 @@ int main(int argc, char **argv)
      * in the constructor above.
      */
 // %Tag(PUBLISH)%
-    chatter_pub.publish(msg);
+    rc_channels_pub.publish(msg);
 // %EndTag(PUBLISH)%
 
 // %Tag(SPINONCE)%
@@ -134,4 +134,4 @@ int main(int argc, char **argv)
 
   return 0;
 }
-// %EndTag(FULLTEXT)%
\ No newline at end of file
+// %EndTag(FULLTEXT)%
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index e46055306c..da69e6e25e 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -28,14 +28,15 @@
 // %Tag(FULLTEXT)%
 #include "ros/ros.h"
 #include "std_msgs/String.h"
+#include "px4/rc_channels.h"
 
 /**
  * This tutorial demonstrates simple receipt of messages over the ROS system.
  */
 // %Tag(CALLBACK)%
-void chatterCallback(const std_msgs::String::ConstPtr& msg)
+void rc_channels_callback(const px4::rc_channels::ConstPtr& msg)
 {
-  ROS_INFO("I heard: [%s]", msg->data.c_str());
+  ROS_INFO("I heard: [%lu]", msg->timestamp_last_valid);
 }
 // %EndTag(CALLBACK)%
 
@@ -76,7 +77,7 @@ int main(int argc, char **argv)
    * away the oldest ones.
    */
 // %Tag(SUBSCRIBER)%
-  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
+  ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
 // %EndTag(SUBSCRIBER)%
 
   /**
@@ -90,4 +91,4 @@ int main(int argc, char **argv)
 
   return 0;
 }
-// %EndTag(FULLTEXT)%
\ No newline at end of file
+// %EndTag(FULLTEXT)%

From b5e34eac4faae4b0bc605c6d2d36ed65740c781d Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 5 Nov 2014 15:40:22 +0100
Subject: [PATCH 033/416] fixed typo

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index fee1174582..b07a1a6be7 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -351,7 +351,7 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4
 	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
 	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
 	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_v_att_sp.thrust     = (control_attitude_thrust_referenc(3) -30)*(-1)/30;
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
 
 	// setup rotation matrix
 	math::Matrix<3,3> Rot_sp;

From f36f54c621cb5b36d345c5a26f72e89fc948fa65 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 9 Nov 2014 11:57:34 +0100
Subject: [PATCH 034/416] Restructuring of generic middleware support files,
 wrapping of the main ROS calls, skeletons for publishers / subscribers

---
 CMakeLists.txt                         |  35 ++++---
 makefiles/config_px4fmu-v2_test.mk     |  13 +--
 src/examples/publisher/module.mk       |  42 ++++++++
 src/examples/publisher/publisher.cpp   | 140 +++++++++----------------
 src/examples/subscriber/module.mk      |  42 ++++++++
 src/examples/subscriber/subscriber.cpp |  99 ++++++++---------
 {include => src/include}/px4.h         |  15 ++-
 {platforms => src/platforms}/empty.c   |   0
 src/platforms/nuttx/module.mk          |  42 ++++++++
 src/platforms/nuttx/px4_nuttx_impl.cpp |  75 +++++++++++++
 src/platforms/nuttx/px4_publisher.cpp  |  40 +++++++
 src/platforms/nuttx/px4_subscriber.cpp |  40 +++++++
 src/platforms/px4_defines.h            |  51 +++++++++
 src/platforms/px4_middleware.h         |  95 +++++++++++++++++
 src/platforms/px4_nodehandle.h         |  46 ++++++++
 src/platforms/px4_publisher.h          |  46 ++++++++
 src/platforms/px4_subscriber.h         |  46 ++++++++
 src/platforms/ros/px4_publisher.cpp    |  40 +++++++
 src/platforms/ros/px4_ros_impl.cpp     |  71 +++++++++++++
 src/platforms/ros/px4_subscriber.cpp   |  40 +++++++
 20 files changed, 851 insertions(+), 167 deletions(-)
 create mode 100644 src/examples/publisher/module.mk
 create mode 100644 src/examples/subscriber/module.mk
 rename {include => src/include}/px4.h (90%)
 rename {platforms => src/platforms}/empty.c (100%)
 create mode 100644 src/platforms/nuttx/module.mk
 create mode 100644 src/platforms/nuttx/px4_nuttx_impl.cpp
 create mode 100644 src/platforms/nuttx/px4_publisher.cpp
 create mode 100644 src/platforms/nuttx/px4_subscriber.cpp
 create mode 100644 src/platforms/px4_defines.h
 create mode 100644 src/platforms/px4_middleware.h
 create mode 100644 src/platforms/px4_nodehandle.h
 create mode 100644 src/platforms/px4_publisher.h
 create mode 100644 src/platforms/px4_subscriber.h
 create mode 100644 src/platforms/ros/px4_publisher.cpp
 create mode 100644 src/platforms/ros/px4_ros_impl.cpp
 create mode 100644 src/platforms/ros/px4_subscriber.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index de2907e6ab..36c2ffeff8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,10 +80,10 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-  INCLUDE_DIRS include
-#  LIBRARIES px4
-#  CATKIN_DEPENDS roscpp rospy std_msgs
-#  DEPENDS system_lib
+  INCLUDE_DIRS src/include
+  LIBRARIES px4
+  CATKIN_DEPENDS roscpp rospy std_msgs
+  DEPENDS system_lib
   CATKIN_DEPENDS message_runtime
 )
 
@@ -93,15 +93,22 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
+  src/platforms
+  src/include
 )
 
 ## Declare a cpp library
-# add_library(px4
-#   src/${PROJECT_NAME}/px4test.cpp # src/platforms/ros/ros.cpp
-# )
+add_library(px4
+  src/platforms/ros/px4_ros_impl.cpp
+  src/platforms/ros/px4_publisher.cpp
+  src/platforms/ros/px4_subscriber.cpp
+)
+
+target_link_libraries(px4
+  ${catkin_LIBRARIES}
+)
 
 ## Declare a test publisher
 add_executable(publisher src/examples/publisher/publisher.cpp)
@@ -113,6 +120,7 @@ add_dependencies(publisher px4_generate_messages_cpp)
 ## Specify libraries to link a library or executable target against
 target_link_libraries(publisher
   ${catkin_LIBRARIES}
+  px4
 )
 
 ## Declare a test subscriber
@@ -125,6 +133,7 @@ add_dependencies(subscriber px4_generate_messages_cpp)
 ## Specify libraries to link a library or executable target against
 target_link_libraries(subscriber
   ${catkin_LIBRARIES}
+  px4
 )
 
 #############
@@ -142,11 +151,11 @@ target_link_libraries(subscriber
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS px4 mc_attitude_control
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS px4 publisher subscriber
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 2f4d9d6a47..67934b8e46 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -34,9 +34,11 @@ MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
 
 #
-# Testing modules
+# Example modules
 #
 MODULES		+= examples/matlab_csv_serial
+MODULES		+= examples/subscriber
+MODULES		+= examples/publisher
 
 #
 # Library modules
@@ -44,16 +46,15 @@ MODULES		+= examples/matlab_csv_serial
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB
-LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/conversion
-
-#
-# Libraries
-#
 LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= platforms/nuttx
 
+#
+# Tests
+#
 MODULES 	+= modules/unit_test
 MODULES		+= modules/mavlink/mavlink_tests
 MODULES 	+= modules/commander/commander_tests
diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk
new file mode 100644
index 0000000000..0fc4316ecb
--- /dev/null
+++ b/src/examples/publisher/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Publisher Example Application
+#
+
+MODULE_COMMAND	= publisher
+
+SRCS		= publisher.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 53fe2e9051..91e0631622 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -26,112 +26,70 @@
  */
 
 #include <px4.h>
-
-// %EndTag(ROS_HEADER)%
-// %Tag(MSG_HEADER)%
-#include "std_msgs/String.h"
-// %EndTag(MSG_HEADER)%
-
-#include <sstream>
 #include <px4/rc_channels.h>
 
+#include <sstream>
+
 /**
- * This tutorial demonstrates simple sending of messages over the ROS system.
+ * This tutorial demonstrates simple sending of messages over the PX4 middleware system.
  */
 int main(int argc, char **argv)
 {
-  /**
-   * The ros::init() function needs to see argc and argv so that it can perform
-   * any ROS arguments and name remapping that were provided at the command line. For programmatic
-   * remappings you can use a different version of init() which takes remappings
-   * directly, but for most command-line programs, passing argc and argv is the easiest
-   * way to do it.  The third argument to init() is the name of the node.
-   *
-   * You must call one of the versions of ros::init() before using any other
-   * part of the ROS system.
-   */
-// %Tag(INIT)%
-  ros::init(argc, argv, "px4_publisher");
-// %EndTag(INIT)%
 
-  /**
-   * NodeHandle is the main access point to communications with the ROS system.
-   * The first NodeHandle constructed will fully initialize this node, and the last
-   * NodeHandle destructed will close down the node.
-   */
-// %Tag(NODEHANDLE)%
-  ros::NodeHandle n;
-// %EndTag(NODEHANDLE)%
+	px4::init(argc, argv, "px4_publisher");
 
-  /**
-   * The advertise() function is how you tell ROS that you want to
-   * publish on a given topic name. This invokes a call to the ROS
-   * master node, which keeps a registry of who is publishing and who
-   * is subscribing. After this advertise() call is made, the master
-   * node will notify anyone who is trying to subscribe to this topic name,
-   * and they will in turn negotiate a peer-to-peer connection with this
-   * node.  advertise() returns a Publisher object which allows you to
-   * publish messages on that topic through a call to publish().  Once
-   * all copies of the returned Publisher object are destroyed, the topic
-   * will be automatically unadvertised.
-   *
-   * The second parameter to advertise() is the size of the message queue
-   * used for publishing messages.  If messages are published more quickly
-   * than we can send them, the number here specifies how many messages to
-   * buffer up before throwing some away.
-   */
-// %Tag(PUBLISHER)%
-  ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
-// %EndTag(PUBLISHER)%
+	ros::NodeHandle n;
 
-// %Tag(LOOP_RATE)%
-  ros::Rate loop_rate(10);
-// %EndTag(LOOP_RATE)%
+	/**
+	 * The advertise() function is how you tell ROS that you want to
+	 * publish on a given topic name. This invokes a call to the ROS
+	 * master node, which keeps a registry of who is publishing and who
+	 * is subscribing. After this advertise() call is made, the master
+	 * node will notify anyone who is trying to subscribe to this topic name,
+	 * and they will in turn negotiate a peer-to-peer connection with this
+	 * node.  advertise() returns a Publisher object which allows you to
+	 * publish messages on that topic through a call to publish().  Once
+	 * all copies of the returned Publisher object are destroyed, the topic
+	 * will be automatically unadvertised.
+	 *
+	 * The second parameter to advertise() is the size of the message queue
+	 * used for publishing messages.  If messages are published more quickly
+	 * than we can send them, the number here specifies how many messages to
+	 * buffer up before throwing some away.
+	 */
+	ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
 
-  /**
-   * A count of how many messages we have sent. This is used to create
-   * a unique string for each message.
-   */
-// %Tag(ROS_OK)%
-  int count = 0;
-  while (ros::ok())
-  {
-// %EndTag(ROS_OK)%
-    /**
-     * This is a message object. You stuff it with data, and then publish it.
-     */
-// %Tag(FILL_MESSAGE)%
-    px4::rc_channels msg;
+	px4::Rate loop_rate(10);
 
-    ros::Time time = ros::Time::now();
-    msg.timestamp_last_valid = time.sec * 1e6 + time.nsec;
-// %EndTag(FILL_MESSAGE)%
+	/**
+	 * A count of how many messages we have sent. This is used to create
+	 * a unique string for each message.
+	 */
+	int count = 0;
 
-// %Tag(ROSCONSOLE)%
-   px4_warnx("%lu", msg.timestamp_last_valid);
-// %EndTag(ROSCONSOLE)%
+	while (px4::ok()) {
+		/**
+		 * This is a message object. You stuff it with data, and then publish it.
+		 */
+		px4::rc_channels msg;
 
-    /**
-     * The publish() function is how you send messages. The parameter
-     * is the message object. The type of this object must agree with the type
-     * given as a template parameter to the advertise<>() call, as was done
-     * in the constructor above.
-     */
-// %Tag(PUBLISH)%
-    rc_channels_pub.publish(msg);
-// %EndTag(PUBLISH)%
+		msg.timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("%lu", msg.timestamp_last_valid);
 
-// %Tag(SPINONCE)%
-    ros::spinOnce();
-// %EndTag(SPINONCE)%
+		/**
+		 * The publish() function is how you send messages. The parameter
+		 * is the message object. The type of this object must agree with the type
+		 * given as a template parameter to the advertise<>() call, as was done
+		 * in the constructor above.
+		 */
+		rc_channels_pub.publish(msg);
 
-// %Tag(RATE_SLEEP)%
-    loop_rate.sleep();
-// %EndTag(RATE_SLEEP)%
-    ++count;
-  }
+		px4::spin_once();
+
+		loop_rate.sleep();
+		++count;
+	}
 
 
-  return 0;
+	return 0;
 }
-// %EndTag(FULLTEXT)%
diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
new file mode 100644
index 0000000000..44d3d03a92
--- /dev/null
+++ b/src/examples/subscriber/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Subscriber Example Application
+#
+
+MODULE_COMMAND	= subscriber
+
+SRCS		= subscriber.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index da69e6e25e..bf16bf84e1 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -25,70 +25,63 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-// %Tag(FULLTEXT)%
-#include "ros/ros.h"
-#include "std_msgs/String.h"
+#include <px4.h>
 #include "px4/rc_channels.h"
 
 /**
- * This tutorial demonstrates simple receipt of messages over the ROS system.
+ * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  */
-// %Tag(CALLBACK)%
-void rc_channels_callback(const px4::rc_channels::ConstPtr& msg)
+void rc_channels_callback(const px4::rc_channels::ConstPtr &msg)
 {
-  ROS_INFO("I heard: [%lu]", msg->timestamp_last_valid);
+	PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid);
 }
-// %EndTag(CALLBACK)%
+
+PX4_MAIN_FUNCTION(subscriber)
 
 int main(int argc, char **argv)
 {
-  /**
-   * The ros::init() function needs to see argc and argv so that it can perform
-   * any ROS arguments and name remapping that were provided at the command line. For programmatic
-   * remappings you can use a different version of init() which takes remappings
-   * directly, but for most command-line programs, passing argc and argv is the easiest
-   * way to do it.  The third argument to init() is the name of the node.
-   *
-   * You must call one of the versions of ros::init() before using any other
-   * part of the ROS system.
-   */
-  ros::init(argc, argv, "listener");
+	/**
+	 * The ros::init() function needs to see argc and argv so that it can perform
+	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
+	 * remappings you can use a different version of init() which takes remappings
+	 * directly, but for most command-line programs, passing argc and argv is the easiest
+	 * way to do it.  The third argument to init() is the name of the node.
+	 *
+	 * You must call one of the versions of px4::init() before using any other
+	 * part of the PX4/ ROS system.
+	 */
+	px4::init(argc, argv, "listener");
 
-  /**
-   * NodeHandle is the main access point to communications with the ROS system.
-   * The first NodeHandle constructed will fully initialize this node, and the last
-   * NodeHandle destructed will close down the node.
-   */
-  ros::NodeHandle n;
+	/**
+	 * NodeHandle is the main access point to communications with the ROS system.
+	 * The first NodeHandle constructed will fully initialize this node, and the last
+	 * NodeHandle destructed will close down the node.
+	 */
+	ros::NodeHandle n;
 
-  /**
-   * The subscribe() call is how you tell ROS that you want to receive messages
-   * on a given topic.  This invokes a call to the ROS
-   * master node, which keeps a registry of who is publishing and who
-   * is subscribing.  Messages are passed to a callback function, here
-   * called chatterCallback.  subscribe() returns a Subscriber object that you
-   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
-   * object go out of scope, this callback will automatically be unsubscribed from
-   * this topic.
-   *
-   * The second parameter to the subscribe() function is the size of the message
-   * queue.  If messages are arriving faster than they are being processed, this
-   * is the number of messages that will be buffered up before beginning to throw
-   * away the oldest ones.
-   */
-// %Tag(SUBSCRIBER)%
-  ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
-// %EndTag(SUBSCRIBER)%
+	/**
+	 * The subscribe() call is how you tell ROS that you want to receive messages
+	 * on a given topic.  This invokes a call to the ROS
+	 * master node, which keeps a registry of who is publishing and who
+	 * is subscribing.  Messages are passed to a callback function, here
+	 * called chatterCallback.  subscribe() returns a Subscriber object that you
+	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
+	 * object go out of scope, this callback will automatically be unsubscribed from
+	 * this topic.
+	 *
+	 * The second parameter to the subscribe() function is the size of the message
+	 * queue.  If messages are arriving faster than they are being processed, this
+	 * is the number of messages that will be buffered up before beginning to throw
+	 * away the oldest ones.
+	 */
+	ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
 
-  /**
-   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
-   * callbacks will be called from within this thread (the main one).  ros::spin()
-   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
-   */
-// %Tag(SPIN)%
-  ros::spin();
-// %EndTag(SPIN)%
+	/**
+	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
+	 * callbacks will be called from within this thread (the main one).  px4::spin()
+	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
+	 */
+	px4::spin();
 
-  return 0;
+	return 0;
 }
-// %EndTag(FULLTEXT)%
diff --git a/include/px4.h b/src/include/px4.h
similarity index 90%
rename from include/px4.h
rename to src/include/px4.h
index 4684cd256f..0aba2ee77d 100644
--- a/include/px4.h
+++ b/src/include/px4.h
@@ -37,17 +37,24 @@
  * Main system header with common convenience functions
  */
 
+#pragma once
+
+#include <stdbool.h>
+
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /*
  * Building for running within the ROS environment
  */
 #include "ros/ros.h"
-#define px4_warnx ROS_WARN
-#define px4_infox ROS_INFO
+#define PX4_WARN ROS_WARN
+#define PX4_INFO ROS_INFO
 #else
 /*
  * Building for NuttX
  */
-#define px4_warnx warnx
-#define px4_infox warnx
+#define PX4_WARN warnx
+#define PX4_INFO warnx
 #endif
+
+#include "../platforms/px4_defines.h"
+#include "../platforms/px4_middleware.h"
diff --git a/platforms/empty.c b/src/platforms/empty.c
similarity index 100%
rename from platforms/empty.c
rename to src/platforms/empty.c
diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk
new file mode 100644
index 0000000000..128f0e7346
--- /dev/null
+++ b/src/platforms/nuttx/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# NuttX / uORB adapter library
+#
+
+SRCS		 = 	px4_nuttx_impl.cpp \
+			px4_publisher.cpp \
+			px4_subscriber.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
new file mode 100644
index 0000000000..3a65297169
--- /dev/null
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nuttx_impl.cpp
+ *
+ * PX4 Middleware Wrapper NuttX Implementation
+ */
+
+#include <px4.h>
+
+extern bool task_should_exit;
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name)
+{
+	px4_warn("process: %s", process_name);
+	return 0;
+}
+
+uint64_t get_time_micros()
+{
+	return hrt_absolute_time();
+}
+
+bool ok()
+{
+	return !task_should_exit;
+}
+
+void spin_once()
+{
+	// XXX check linked list of topics with orb_check() here
+
+}
+
+void spin()
+{
+	// XXX block waiting for updated topics here
+
+}
+
+}
diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp
new file mode 100644
index 0000000000..ab6035b225
--- /dev/null
+++ b/src/platforms/nuttx/px4_publisher.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_publisher.cpp
+ *
+ * PX4 Middleware Wrapper for Publisher
+ */
+
+
diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp
new file mode 100644
index 0000000000..088c08fdb6
--- /dev/null
+++ b/src/platforms/nuttx/px4_subscriber.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_subscriber.cpp
+ *
+ * PX4 Middleware Wrapper Subscriber
+ */
+
+
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
new file mode 100644
index 0000000000..48234766f0
--- /dev/null
+++ b/src/platforms/px4_defines.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_defines.h
+ *
+ * Generally used magic defines
+ */
+
+#pragma once
+
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/*
+ * Building for running within the ROS environment
+ */
+#define __EXPORT
+#define PX4_MAIN_FUNCTION(_prefix)
+#else
+#include <nuttx/config.h>
+#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }
+#endif
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
new file mode 100644
index 0000000000..d1c0656af8
--- /dev/null
+++ b/src/platforms/px4_middleware.h
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_middleware.h
+ *
+ * PX4 generic middleware wrapper
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name);
+
+uint64_t get_time_micros();
+
+bool ok();
+
+void spin_once();
+
+void spin();
+
+class Rate
+{
+
+public:
+	explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
+
+	void sleep() { usleep(sleep_interval); }
+
+private:
+	uint64_t sleep_interval;
+
+};
+
+// /**
+//  * A limiter/ saturation.
+//  * The output of update is the input, bounded
+//  * by min/max.
+//  */
+// class __EXPORT BlockLimit : public Block
+// {
+// public:
+// // methods
+// 	BlockLimit(SuperBlock *parent, const char *name) :
+// 		Block(parent, name),
+// 		_min(this, "MIN"),
+// 		_max(this, "MAX")
+// 	{};
+// 	virtual ~BlockLimit() {};
+// 	float update(float input);
+// // accessors
+// 	float getMin() { return _min.get(); }
+// 	float getMax() { return _max.get(); }
+// protected:
+// // attributes
+// 	control::BlockParamFloat _min;
+// 	control::BlockParamFloat _max;
+// };
+
+} // namespace px4
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
new file mode 100644
index 0000000000..d278828b7c
--- /dev/null
+++ b/src/platforms/px4_nodehandle.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.h
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+
+namespace px4
+{
+class NodeHandle
+{
+
+};
+}
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
new file mode 100644
index 0000000000..1b09521554
--- /dev/null
+++ b/src/platforms/px4_publisher.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.h
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+
+namespace px4
+{
+class Publisher
+{
+
+};
+}
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
new file mode 100644
index 0000000000..8759f8b05d
--- /dev/null
+++ b/src/platforms/px4_subscriber.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_subscriber.h
+ *
+ * PX4 Middleware Wrapper Subscriber
+ */
+
+namespace px4
+{
+class Subscriber
+{
+
+};
+}
diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp
new file mode 100644
index 0000000000..ab6035b225
--- /dev/null
+++ b/src/platforms/ros/px4_publisher.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_publisher.cpp
+ *
+ * PX4 Middleware Wrapper for Publisher
+ */
+
+
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
new file mode 100644
index 0000000000..eda17e5a91
--- /dev/null
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_ros_impl.cpp
+ *
+ * PX4 Middleware Wrapper ROS Implementation
+ */
+
+#include <px4.h>
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name)
+{
+	ros::init(argc, argv, process_name);
+}
+
+uint64_t get_time_micros()
+{
+	ros::Time time = ros::Time::now();
+	return time.sec * 1e6 + time.nsec / 1000;
+}
+
+bool ok()
+{
+	return ros::ok();
+}
+
+void spin_once()
+{
+	ros::spinOnce();
+}
+
+void spin()
+{
+	ros::spin();
+}
+
+}
diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp
new file mode 100644
index 0000000000..088c08fdb6
--- /dev/null
+++ b/src/platforms/ros/px4_subscriber.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_subscriber.cpp
+ *
+ * PX4 Middleware Wrapper Subscriber
+ */
+
+

From c9a7850b93643eee7a7ddb145972c4cb36f5db66 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 16:10:01 +0100
Subject: [PATCH 035/416] do not include eigen lib from local source

---
 src/lib/mathlib/math/Matrix.hpp | 2 +-
 src/lib/mathlib/math/Vector.hpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 34fc4604ac..832be66dbf 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -50,7 +50,7 @@
 #include "../CMSIS/Include/arm_math.h"
 #else
 #include <math/eigen_math.h>
-#include <eigen/Eigen/Dense>
+#include <Eigen/Eigen>
 #define M_PI_2_F 1.570769
 #endif
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 51a84d27eb..b0b03980d3 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -50,7 +50,7 @@
 #include "../CMSIS/Include/arm_math.h"
 #else
 #include <math/eigen_math.h>
-#include <eigen/Eigen/Dense>
+#include <Eigen/Eigen>
 #endif
 
 namespace math

From f72f0db9963049879eb0dce9a9ef37baadd45dd2 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 16:10:43 +0100
Subject: [PATCH 036/416] include correct eigen lib header

---
 src/modules/mc_att_control/mc_att_control_base.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 8f1cf015ea..5a08bba798 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -76,7 +76,7 @@
 #include <systemlib/perf_counter.h>
 //#include <systemlib/systemlib.h>
 #include <lib/mathlib/mathlib.h>
-#include <Eigen/Eigen>
+//#include <Eigen/Eigen>
 
 
 

From a2ec2ec857f5b7584628c2a923c85d0b62b082dd Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 16:58:56 +0100
Subject: [PATCH 037/416] cleaned up

---
 .../mc_att_control/mc_att_control_base.h      | 58 ++-----------------
 1 file changed, 6 insertions(+), 52 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 5a08bba798..515fb0c14a 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -1,14 +1,7 @@
-/*
- * mc_att_control_base.h
- *
- *  Created on: Sep 25, 2014
- *      Author: roman
- */
-
 #ifndef MC_ATT_CONTROL_BASE_H_
 #define MC_ATT_CONTROL_BASE_H_
 
-/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -40,21 +33,10 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_main.cpp
- * Multicopter attitude controller.
+ * @file mc_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- *
- * The controller has two loops: P loop for angular error and PD loop for angular rate error.
- * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
- * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
- * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
- * These two approaches fused seamlessly with weight depending on angular error.
- * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
- * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
- * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
 #include <stdio.h>
@@ -74,18 +56,10 @@
 #include <uORB/topics/actuator_armed.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
-//#include <systemlib/systemlib.h>
 #include <lib/mathlib/mathlib.h>
-//#include <Eigen/Eigen>
 
 
 
-/**
- * Multicopter attitude control app start / stop handling function
- *
- * @ingroup apps
- */
-
 #define YAW_DEADZONE	0.05f
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
@@ -98,7 +72,7 @@ public:
 	MulticopterAttitudeControlBase();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
 	~MulticopterAttitudeControlBase();
 
@@ -109,6 +83,7 @@ public:
 	 */
 	void control_attitude(float dt);
 	void control_attitude_rates(float dt);
+	
 	 // setters and getters for interface with euroc-gazebo simulator
 	void set_attitude(const Eigen::Quaternion<double> attitude);
 	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
@@ -120,10 +95,6 @@ protected:
 
 	bool _task_should_exit; /**< if true, sensor task should exit */
 	int _control_task; /**< task handle for sensor task */
-
-
-
-
 	bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
 
 	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
@@ -146,8 +117,6 @@ protected:
 
 	bool _reset_yaw_sp; /**< reset yaw setpoint flag */
 
-
-
 	struct {
 		math::Vector<3> att_p; /**< P gain for angular error */
 		math::Vector<3> rate_p; /**< P gain for angular rate error */
@@ -161,24 +130,9 @@ protected:
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
 	} _params;
-
-
-	/**
-	 * Attitude controller.
-	 */
-	//void control_attitude(float dt);
-
-	/**
-	 * Attitude rates controller.
-	 */
 	
 	void vehicle_attitude_setpoint_poll();	//provisional
 
-	
-	
-
-
-
 };
 
 #endif /* MC_ATT_CONTROL_BASE_H_ */

From 0acdf5927b9e2dd2cc7375008af2d00cd7f9ba0e Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 17:01:59 +0100
Subject: [PATCH 038/416] cleaned up

---
 .../mc_att_control/mc_att_control_base.cpp    | 44 +++++++++++++++----
 1 file changed, 35 insertions(+), 9 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index b07a1a6be7..baf2bfe65c 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -1,8 +1,39 @@
-/*
- * mc_att_control_base.cpp
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
- *  Created on: Sep 25, 2014
- *      Author: roman
  */
 
 #include "mc_att_control_base.h"
@@ -308,11 +339,9 @@ void MulticopterAttitudeControlBase::set_actuator_controls() {
 	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
 	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
-	//_actuators.timestamp = hrt_absolute_time();
 }
 
 void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
-	// check if this is consistent !!!
 	math::Quaternion quat;
 	quat(0) = (float)attitude.w();
 	quat(1) = (float)attitude.x();
@@ -336,7 +365,6 @@ void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double
 	_v_att.R[2][2] = Rot(2,2);
 
 	_v_att.R_valid = true;
-
 }
 
 void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
@@ -344,7 +372,6 @@ void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& a
 	_v_att.rollspeed  = angular_rate(0);
 	_v_att.pitchspeed = angular_rate(1);
 	_v_att.yawspeed   = angular_rate(2);
-
 }
 
 void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
@@ -365,7 +392,6 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4
 	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
 	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
 	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
-
 }
 
 void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {

From 43ebaf287c86acbf7fba13b5203de68afcc79b44 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 17:06:14 +0100
Subject: [PATCH 039/416] cleaned up

---
 .../fw_att_control/fw_att_control_base.cpp    | 39 ++++++++++--
 .../fw_att_control/fw_att_control_base.h      | 59 +++++++++++++------
 2 files changed, 75 insertions(+), 23 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
index 7218e4e9bb..d8ba159693 100644
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -1,8 +1,39 @@
-/*
- * fw_att_control_base.cpp
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.cpp
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
- *  Created on: Sep 24, 2014
- *      Author: roman
  */
 
 #include "fw_att_control_base.h"
diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h
index 6e071fe201..6b2efc46b2 100644
--- a/src/modules/fw_att_control/fw_att_control_base.h
+++ b/src/modules/fw_att_control/fw_att_control_base.h
@@ -1,13 +1,44 @@
-/*
- * fw_att_control_base.h
- *
- *  Created on: Sep 24, 2014
- *      Author: roman
- */
-
 #ifndef FW_ATT_CONTROL_BASE_H_
 #define FW_ATT_CONTROL_BASE_H_
 
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file fw_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
@@ -22,8 +53,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <drivers/drv_accel.h>
 
+#include <drivers/drv_accel.h>
 #include <systemlib/perf_counter.h>
 
 class FixedwingAttitudeControlBase
@@ -35,7 +66,7 @@ public:
 	FixedwingAttitudeControlBase();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
 	~FixedwingAttitudeControlBase();
 
@@ -46,8 +77,6 @@ protected:
 	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_control_task;			/**< task handle for sensor task */
 
-
-
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
@@ -107,20 +136,12 @@ protected:
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
-
-
-
 	ECL_RollController				_roll_ctrl;
 	ECL_PitchController				_pitch_ctrl;
 	ECL_YawController				_yaw_ctrl;
 
 	void control_attitude();
 
-
-
-
 };
 
-
-
 #endif /* FW_ATT_CONTROL_BASE_H_ */

From 403fe886845b8e3da150bdc45e47c4e4b29cea96 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 17:24:48 +0100
Subject: [PATCH 040/416] deleted folder that has found its way into the repo

---
 build/CATKIN_IGNORE                           |   0
 build/CMakeCache.txt                          | 450 ------------------
 .../CMakeFiles/2.8.12.2/CMakeCCompiler.cmake  |  56 ---
 .../2.8.12.2/CMakeCXXCompiler.cmake           |  57 ---
 .../2.8.12.2/CMakeDetermineCompilerABI_C.bin  | Bin 8547 -> 0 bytes
 .../CMakeDetermineCompilerABI_CXX.bin         | Bin 8560 -> 0 bytes
 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake   |  15 -
 .../2.8.12.2/CompilerIdC/CMakeCCompilerId.c   | 389 ---------------
 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out   | Bin 8643 -> 0 bytes
 .../CompilerIdCXX/CMakeCXXCompilerId.cpp      | 377 ---------------
 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out | Bin 8652 -> 0 bytes
 .../CMakeDirectoryInformation.cmake           |  16 -
 build/CMakeFiles/CMakeError.log               |  53 ---
 build/CMakeFiles/CMakeOutput.log              | 293 ------------
 build/CMakeFiles/CMakeRuleHashes.txt          |   5 -
 build/CMakeFiles/Makefile.cmake               | 150 ------
 build/CMakeFiles/Makefile2                    | 266 -----------
 build/CMakeFiles/TargetDirectories.txt        |   6 -
 .../clean_test_results.dir/DependInfo.cmake   |  20 -
 .../clean_test_results.dir/build.make         |  66 ---
 .../clean_test_results.dir/cmake_clean.cmake  |   8 -
 .../clean_test_results.dir/progress.make      |   1 -
 build/CMakeFiles/cmake.check_cache            |   1 -
 build/CMakeFiles/doxygen.dir/DependInfo.cmake |  20 -
 build/CMakeFiles/doxygen.dir/build.make       |  65 ---
 .../CMakeFiles/doxygen.dir/cmake_clean.cmake  |   8 -
 build/CMakeFiles/doxygen.dir/progress.make    |   1 -
 build/CMakeFiles/progress.marks               |   1 -
 .../CMakeFiles/run_tests.dir/DependInfo.cmake |  20 -
 build/CMakeFiles/run_tests.dir/build.make     |  65 ---
 .../run_tests.dir/cmake_clean.cmake           |   8 -
 build/CMakeFiles/run_tests.dir/progress.make  |   1 -
 build/CMakeFiles/tests.dir/DependInfo.cmake   |  20 -
 build/CMakeFiles/tests.dir/build.make         |  65 ---
 build/CMakeFiles/tests.dir/cmake_clean.cmake  |   8 -
 build/CMakeFiles/tests.dir/progress.make      |   1 -
 build/CTestTestfile.cmake                     |   7 -
 build/Makefile                                | 262 ----------
 .../catkin_generated/version/package.cmake    |   9 -
 build/catkin_generated/env_cached.sh          |  16 -
 .../catkin_generated/generate_cached_setup.py |  29 --
 .../catkin_generated/installspace/.rosinstall |   2 -
 .../installspace/_setup_util.py               | 287 -----------
 build/catkin_generated/installspace/env.sh    |  16 -
 .../catkin_generated/installspace/setup.bash  |   8 -
 build/catkin_generated/installspace/setup.sh  |  87 ----
 build/catkin_generated/installspace/setup.zsh |   8 -
 build/catkin_generated/order_packages.cmake   |  10 -
 build/catkin_generated/order_packages.py      |   5 -
 build/catkin_generated/setup_cached.sh        |  20 -
 .../Project/interrogate_setup_dot_py.py.stamp | 250 ----------
 .../Project/order_packages.cmake.em.stamp     |  56 ---
 .../stamps/Project/package.xml.stamp          |  37 --
 build/catkin_make.cache                       |   1 -
 build/cmake_install.cmake                     | 140 ------
 .../CMakeDirectoryInformation.cmake           |  16 -
 .../CMakeFiles/gtest.dir/DependInfo.cmake     |  27 --
 build/gtest/CMakeFiles/gtest.dir/build.make   | 102 ----
 .../CMakeFiles/gtest.dir/cmake_clean.cmake    |  10 -
 build/gtest/CMakeFiles/gtest.dir/depend.make  |   2 -
 build/gtest/CMakeFiles/gtest.dir/flags.make   |   8 -
 build/gtest/CMakeFiles/gtest.dir/link.txt     |   1 -
 .../gtest/CMakeFiles/gtest.dir/progress.make  |   2 -
 .../gtest_main.dir/DependInfo.cmake           |  28 --
 .../CMakeFiles/gtest_main.dir/build.make      | 103 ----
 .../gtest_main.dir/cmake_clean.cmake          |  10 -
 .../CMakeFiles/gtest_main.dir/depend.make     |   2 -
 .../CMakeFiles/gtest_main.dir/flags.make      |   8 -
 .../gtest/CMakeFiles/gtest_main.dir/link.txt  |   1 -
 .../CMakeFiles/gtest_main.dir/progress.make   |   2 -
 build/gtest/CMakeFiles/progress.marks         |   1 -
 build/gtest/CTestTestfile.cmake               |   6 -
 build/gtest/Makefile                          | 262 ----------
 build/gtest/cmake_install.cmake               |  34 --
 74 files changed, 4387 deletions(-)
 delete mode 100644 build/CATKIN_IGNORE
 delete mode 100644 build/CMakeCache.txt
 delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake
 delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake
 delete mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin
 delete mode 100755 build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin
 delete mode 100644 build/CMakeFiles/2.8.12.2/CMakeSystem.cmake
 delete mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c
 delete mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdC/a.out
 delete mode 100644 build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
 delete mode 100755 build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out
 delete mode 100644 build/CMakeFiles/CMakeDirectoryInformation.cmake
 delete mode 100644 build/CMakeFiles/CMakeError.log
 delete mode 100644 build/CMakeFiles/CMakeOutput.log
 delete mode 100644 build/CMakeFiles/CMakeRuleHashes.txt
 delete mode 100644 build/CMakeFiles/Makefile.cmake
 delete mode 100644 build/CMakeFiles/Makefile2
 delete mode 100644 build/CMakeFiles/TargetDirectories.txt
 delete mode 100644 build/CMakeFiles/clean_test_results.dir/DependInfo.cmake
 delete mode 100644 build/CMakeFiles/clean_test_results.dir/build.make
 delete mode 100644 build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake
 delete mode 100644 build/CMakeFiles/clean_test_results.dir/progress.make
 delete mode 100644 build/CMakeFiles/cmake.check_cache
 delete mode 100644 build/CMakeFiles/doxygen.dir/DependInfo.cmake
 delete mode 100644 build/CMakeFiles/doxygen.dir/build.make
 delete mode 100644 build/CMakeFiles/doxygen.dir/cmake_clean.cmake
 delete mode 100644 build/CMakeFiles/doxygen.dir/progress.make
 delete mode 100644 build/CMakeFiles/progress.marks
 delete mode 100644 build/CMakeFiles/run_tests.dir/DependInfo.cmake
 delete mode 100644 build/CMakeFiles/run_tests.dir/build.make
 delete mode 100644 build/CMakeFiles/run_tests.dir/cmake_clean.cmake
 delete mode 100644 build/CMakeFiles/run_tests.dir/progress.make
 delete mode 100644 build/CMakeFiles/tests.dir/DependInfo.cmake
 delete mode 100644 build/CMakeFiles/tests.dir/build.make
 delete mode 100644 build/CMakeFiles/tests.dir/cmake_clean.cmake
 delete mode 100644 build/CMakeFiles/tests.dir/progress.make
 delete mode 100644 build/CTestTestfile.cmake
 delete mode 100644 build/Makefile
 delete mode 100644 build/catkin/catkin_generated/version/package.cmake
 delete mode 100755 build/catkin_generated/env_cached.sh
 delete mode 100644 build/catkin_generated/generate_cached_setup.py
 delete mode 100644 build/catkin_generated/installspace/.rosinstall
 delete mode 100755 build/catkin_generated/installspace/_setup_util.py
 delete mode 100755 build/catkin_generated/installspace/env.sh
 delete mode 100644 build/catkin_generated/installspace/setup.bash
 delete mode 100644 build/catkin_generated/installspace/setup.sh
 delete mode 100644 build/catkin_generated/installspace/setup.zsh
 delete mode 100644 build/catkin_generated/order_packages.cmake
 delete mode 100644 build/catkin_generated/order_packages.py
 delete mode 100755 build/catkin_generated/setup_cached.sh
 delete mode 100644 build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp
 delete mode 100644 build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp
 delete mode 100644 build/catkin_generated/stamps/Project/package.xml.stamp
 delete mode 100644 build/catkin_make.cache
 delete mode 100644 build/cmake_install.cmake
 delete mode 100644 build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/build.make
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/depend.make
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/flags.make
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/link.txt
 delete mode 100644 build/gtest/CMakeFiles/gtest.dir/progress.make
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/build.make
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/depend.make
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/flags.make
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/link.txt
 delete mode 100644 build/gtest/CMakeFiles/gtest_main.dir/progress.make
 delete mode 100644 build/gtest/CMakeFiles/progress.marks
 delete mode 100644 build/gtest/CTestTestfile.cmake
 delete mode 100644 build/gtest/Makefile
 delete mode 100644 build/gtest/cmake_install.cmake

diff --git a/build/CATKIN_IGNORE b/build/CATKIN_IGNORE
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt
deleted file mode 100644
index 8fc5c4b4a8..0000000000
--- a/build/CMakeCache.txt
+++ /dev/null
@@ -1,450 +0,0 @@
-# This is the CMakeCache file.
-# For build in directory: /home/roman/src/Firmware/build
-# It was generated by CMake: /usr/bin/cmake
-# You can edit this file to change values found and used by cmake.
-# If you do not want to change any of the values, simply exit the editor.
-# If you do want to change a value, simply edit, save, and exit the editor.
-# The syntax for the file is as follows:
-# KEY:TYPE=VALUE
-# KEY is the name of a variable in the cache.
-# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!.
-# VALUE is the current value for the KEY.
-
-########################
-# EXTERNAL cache entries
-########################
-
-//Build shared libraries (DLLs).
-BUILD_SHARED_LIBS:BOOL=ON
-
-//List of ';' separated packages to exclude
-CATKIN_BLACKLIST_PACKAGES:STRING=
-
-//catkin devel space
-CATKIN_DEVEL_PREFIX:PATH=/home/roman/src/Firmware/devel
-
-//Catkin enable testing
-CATKIN_ENABLE_TESTING:BOOL=ON
-
-//Catkin skip testing
-CATKIN_SKIP_TESTING:BOOL=OFF
-
-//List of ';' separated packages to build
-CATKIN_WHITELIST_PACKAGES:STRING=
-
-//Path to a program.
-CMAKE_AR:FILEPATH=/usr/bin/ar
-
-//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or
-// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel.
-CMAKE_BUILD_TYPE:STRING=
-
-//Enable/Disable color output during build.
-CMAKE_COLOR_MAKEFILE:BOOL=ON
-
-//CXX compiler.
-CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++
-
-//Flags used by the compiler during all build types.
-CMAKE_CXX_FLAGS:STRING=
-
-//Flags used by the compiler during debug builds.
-CMAKE_CXX_FLAGS_DEBUG:STRING=-g
-
-//Flags used by the compiler during release minsize builds.
-CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
-
-//Flags used by the compiler during release builds (/MD /Ob1 /Oi
-// /Ot /Oy /Gs will produce slightly less optimized but smaller
-// files).
-CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
-
-//Flags used by the compiler during Release with Debug Info builds.
-CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
-
-//C compiler.
-CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc
-
-//Flags used by the compiler during all build types.
-CMAKE_C_FLAGS:STRING=
-
-//Flags used by the compiler during debug builds.
-CMAKE_C_FLAGS_DEBUG:STRING=-g
-
-//Flags used by the compiler during release minsize builds.
-CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
-
-//Flags used by the compiler during release builds (/MD /Ob1 /Oi
-// /Ot /Oy /Gs will produce slightly less optimized but smaller
-// files).
-CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
-
-//Flags used by the compiler during Release with Debug Info builds.
-CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
-
-//Flags used by the linker.
-CMAKE_EXE_LINKER_FLAGS:STRING=' '
-
-//Flags used by the linker during debug builds.
-CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Enable/Disable output of compile commands during generation.
-CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF
-
-//Install path prefix, prepended onto install directories.
-CMAKE_INSTALL_PREFIX:PATH=/home/roman/src/Firmware/install
-
-//Path to a program.
-CMAKE_LINKER:FILEPATH=/usr/bin/ld
-
-//Path to a program.
-CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make
-
-//Flags used by the linker during the creation of modules.
-CMAKE_MODULE_LINKER_FLAGS:STRING=' '
-
-//Flags used by the linker during debug builds.
-CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Path to a program.
-CMAKE_NM:FILEPATH=/usr/bin/nm
-
-//Path to a program.
-CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy
-
-//Path to a program.
-CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump
-
-//Value Computed by CMake
-CMAKE_PROJECT_NAME:STATIC=Project
-
-//Path to a program.
-CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib
-
-//Flags used by the linker during the creation of dll's.
-CMAKE_SHARED_LINKER_FLAGS:STRING=' '
-
-//Flags used by the linker during debug builds.
-CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//If set, runtime paths are not added when installing shared libraries,
-// but are added when building.
-CMAKE_SKIP_INSTALL_RPATH:BOOL=NO
-
-//If set, runtime paths are not added when using shared libraries.
-CMAKE_SKIP_RPATH:BOOL=NO
-
-//Flags used by the linker during the creation of static libraries.
-CMAKE_STATIC_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during debug builds.
-CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Path to a program.
-CMAKE_STRIP:FILEPATH=/usr/bin/strip
-
-//If true, cmake will use relative paths in makefiles and projects.
-CMAKE_USE_RELATIVE_PATHS:BOOL=OFF
-
-//If this value is on, makefiles will be generated without the
-// .SILENT directive, and all commands will be echoed to the console
-// during the make.  This is useful for debugging only. With Visual
-// Studio IDE projects all commands are done without /nologo.
-CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE
-
-//Path to a program.
-DOXYGEN_EXECUTABLE:FILEPATH=DOXYGEN_EXECUTABLE-NOTFOUND
-
-//Path to a program.
-EMPY_EXECUTABLE:FILEPATH=/usr/bin/empy
-
-//Empy script
-EMPY_SCRIPT:STRING=/usr/bin/empy
-
-//Path to a file.
-GTEST_INCLUDE_DIR:PATH=/usr/include
-
-//Path to a library.
-GTEST_LIBRARY:FILEPATH=GTEST_LIBRARY-NOTFOUND
-
-//Path to a library.
-GTEST_LIBRARY_DEBUG:FILEPATH=GTEST_LIBRARY_DEBUG-NOTFOUND
-
-//Path to a library.
-GTEST_MAIN_LIBRARY:FILEPATH=GTEST_MAIN_LIBRARY-NOTFOUND
-
-//Path to a library.
-GTEST_MAIN_LIBRARY_DEBUG:FILEPATH=GTEST_MAIN_LIBRARY_DEBUG-NOTFOUND
-
-//lsb_release executable was found
-LSB_FOUND:BOOL=TRUE
-
-//Path to a program.
-LSB_RELEASE_EXECUTABLE:FILEPATH=/usr/bin/lsb_release
-
-//Path to a program.
-NOSETESTS:FILEPATH=/usr/bin/nosetests-2.7
-
-//Path to a program.
-PYTHON_EXECUTABLE:FILEPATH=/usr/bin/python
-
-//Specify specific Python version to use ('major.minor' or 'major')
-PYTHON_VERSION:STRING=
-
-//Value Computed by CMake
-Project_BINARY_DIR:STATIC=/home/roman/src/Firmware/build
-
-//Value Computed by CMake
-Project_SOURCE_DIR:STATIC=/home/roman/src/Firmware/src
-
-//Path to a library.
-RT_LIBRARY:FILEPATH=/usr/lib/x86_64-linux-gnu/librt.so
-
-//Enable debian style python package layout
-SETUPTOOLS_DEB_LAYOUT:BOOL=ON
-
-//LSB Distrib tag
-UBUNTU:BOOL=TRUE
-
-//LSB Distrib - codename tag
-UBUNTU_TRUSTY:BOOL=TRUE
-
-//Path to a file.
-_CATKIN_GTEST_INCLUDE:FILEPATH=/usr/include/gtest/gtest.h
-
-//Path to a file.
-_CATKIN_GTEST_SRC:FILEPATH=/usr/src/gtest/src/gtest.cc
-
-//The directory containing a CMake configuration file for catkin.
-catkin_DIR:PATH=/opt/ros/indigo/share/catkin/cmake
-
-//Value Computed by CMake
-gtest_BINARY_DIR:STATIC=/home/roman/src/Firmware/build/gtest
-
-//Dependencies for the target
-gtest_LIB_DEPENDS:STATIC=general;-lpthread;
-
-//Value Computed by CMake
-gtest_SOURCE_DIR:STATIC=/usr/src/gtest
-
-//Build gtest's sample programs.
-gtest_build_samples:BOOL=OFF
-
-//Build all of gtest's own tests.
-gtest_build_tests:BOOL=OFF
-
-//Disable uses of pthreads in gtest.
-gtest_disable_pthreads:BOOL=OFF
-
-//Use shared (DLL) run-time lib even when Google Test is built
-// as static lib.
-gtest_force_shared_crt:BOOL=OFF
-
-//Dependencies for the target
-gtest_main_LIB_DEPENDS:STATIC=general;-lpthread;general;gtest;
-
-
-########################
-# INTERNAL cache entries
-########################
-
-//catkin environment
-CATKIN_ENV:INTERNAL=/home/roman/src/Firmware/build/catkin_generated/env_cached.sh
-CATKIN_TEST_RESULTS_DIR:INTERNAL=/home/roman/src/Firmware/build/test_results
-//ADVANCED property for variable: CMAKE_AR
-CMAKE_AR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_BUILD_TOOL
-CMAKE_BUILD_TOOL-ADVANCED:INTERNAL=1
-//What is the target build tool cmake is generating for.
-CMAKE_BUILD_TOOL:INTERNAL=/usr/bin/make
-//This is the directory where this CMakeCache.txt was created
-CMAKE_CACHEFILE_DIR:INTERNAL=/home/roman/src/Firmware/build
-//Major version of cmake used to create the current loaded cache
-CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2
-//Minor version of cmake used to create the current loaded cache
-CMAKE_CACHE_MINOR_VERSION:INTERNAL=8
-//Patch version of cmake used to create the current loaded cache
-CMAKE_CACHE_PATCH_VERSION:INTERNAL=12
-//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE
-CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1
-//Path to CMake executable.
-CMAKE_COMMAND:INTERNAL=/usr/bin/cmake
-//Path to cpack program executable.
-CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack
-//Path to ctest program executable.
-CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest
-//ADVANCED property for variable: CMAKE_CXX_COMPILER
-CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS
-CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG
-CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL
-CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE
-CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO
-CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER
-CMAKE_C_COMPILER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS
-CMAKE_C_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG
-CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL
-CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE
-CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO
-CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//Executable file format
-CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS
-CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG
-CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL
-CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE
-CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS
-CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1
-//Name of generator.
-CMAKE_GENERATOR:INTERNAL=Unix Makefiles
-//Name of generator toolset.
-CMAKE_GENERATOR_TOOLSET:INTERNAL=
-//Have symbol pthread_create
-CMAKE_HAVE_LIBC_CREATE:INTERNAL=
-//Have library pthreads
-CMAKE_HAVE_PTHREADS_CREATE:INTERNAL=
-//Have library pthread
-CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1
-//Have include pthread.h
-CMAKE_HAVE_PTHREAD_H:INTERNAL=1
-//Start directory with the top level CMakeLists.txt file for this
-// project
-CMAKE_HOME_DIRECTORY:INTERNAL=/home/roman/src/Firmware/src
-//Install .so files without execute permission.
-CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1
-//ADVANCED property for variable: CMAKE_LINKER
-CMAKE_LINKER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MAKE_PROGRAM
-CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS
-CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG
-CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL
-CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE
-CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_NM
-CMAKE_NM-ADVANCED:INTERNAL=1
-//number of local generators
-CMAKE_NUMBER_OF_LOCAL_GENERATORS:INTERNAL=2
-//ADVANCED property for variable: CMAKE_OBJCOPY
-CMAKE_OBJCOPY-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_OBJDUMP
-CMAKE_OBJDUMP-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_RANLIB
-CMAKE_RANLIB-ADVANCED:INTERNAL=1
-//Path to CMake installation.
-CMAKE_ROOT:INTERNAL=/usr/share/cmake-2.8
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS
-CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG
-CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL
-CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE
-CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH
-CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SKIP_RPATH
-CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS
-CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG
-CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL
-CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE
-CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STRIP
-CMAKE_STRIP-ADVANCED:INTERNAL=1
-//uname command
-CMAKE_UNAME:INTERNAL=/bin/uname
-//ADVANCED property for variable: CMAKE_USE_RELATIVE_PATHS
-CMAKE_USE_RELATIVE_PATHS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE
-CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1
-//Details about finding PythonInterp
-FIND_PACKAGE_MESSAGE_DETAILS_PythonInterp:INTERNAL=[/usr/bin/python][v2.7.6()]
-//Details about finding Threads
-FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()]
-GTEST_FROM_SOURCE_FOUND:INTERNAL=TRUE
-GTEST_FROM_SOURCE_INCLUDE_DIRS:INTERNAL=/usr/include
-GTEST_FROM_SOURCE_LIBRARIES:INTERNAL=gtest
-GTEST_FROM_SOURCE_LIBRARY_DIRS:INTERNAL=/home/roman/src/Firmware/build/gtest
-GTEST_FROM_SOURCE_MAIN_LIBRARIES:INTERNAL=gtest_main
-//ADVANCED property for variable: GTEST_INCLUDE_DIR
-GTEST_INCLUDE_DIR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: GTEST_LIBRARY
-GTEST_LIBRARY-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: GTEST_LIBRARY_DEBUG
-GTEST_LIBRARY_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: GTEST_MAIN_LIBRARY
-GTEST_MAIN_LIBRARY-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: GTEST_MAIN_LIBRARY_DEBUG
-GTEST_MAIN_LIBRARY_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: PYTHON_EXECUTABLE
-PYTHON_EXECUTABLE-ADVANCED:INTERNAL=1
-//This needs to be in PYTHONPATH when 'setup.py install' is called.
-//  And it needs to match.  But setuptools won't tell us where
-// it will install things.
-PYTHON_INSTALL_DIR:INTERNAL=lib/python2.7/dist-packages
-
diff --git a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake
deleted file mode 100644
index 83254ce491..0000000000
--- a/build/CMakeFiles/2.8.12.2/CMakeCCompiler.cmake
+++ /dev/null
@@ -1,56 +0,0 @@
-set(CMAKE_C_COMPILER "/usr/bin/cc")
-set(CMAKE_C_COMPILER_ARG1 "")
-set(CMAKE_C_COMPILER_ID "GNU")
-set(CMAKE_C_COMPILER_VERSION "4.8.2")
-set(CMAKE_C_PLATFORM_ID "Linux")
-
-set(CMAKE_AR "/usr/bin/ar")
-set(CMAKE_RANLIB "/usr/bin/ranlib")
-set(CMAKE_LINKER "/usr/bin/ld")
-set(CMAKE_COMPILER_IS_GNUCC 1)
-set(CMAKE_C_COMPILER_LOADED 1)
-set(CMAKE_C_COMPILER_WORKS TRUE)
-set(CMAKE_C_ABI_COMPILED TRUE)
-set(CMAKE_COMPILER_IS_MINGW )
-set(CMAKE_COMPILER_IS_CYGWIN )
-if(CMAKE_COMPILER_IS_CYGWIN)
-  set(CYGWIN 1)
-  set(UNIX 1)
-endif()
-
-set(CMAKE_C_COMPILER_ENV_VAR "CC")
-
-if(CMAKE_COMPILER_IS_MINGW)
-  set(MINGW 1)
-endif()
-set(CMAKE_C_COMPILER_ID_RUN 1)
-set(CMAKE_C_SOURCE_FILE_EXTENSIONS c)
-set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC)
-set(CMAKE_C_LINKER_PREFERENCE 10)
-
-# Save compiler ABI information.
-set(CMAKE_C_SIZEOF_DATA_PTR "8")
-set(CMAKE_C_COMPILER_ABI "ELF")
-set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-
-if(CMAKE_C_SIZEOF_DATA_PTR)
-  set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}")
-endif()
-
-if(CMAKE_C_COMPILER_ABI)
-  set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}")
-endif()
-
-if(CMAKE_C_LIBRARY_ARCHITECTURE)
-  set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-endif()
-
-
-
-
-set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "c")
-set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib")
-set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "")
-
-
-
diff --git a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake b/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake
deleted file mode 100644
index c4373d570c..0000000000
--- a/build/CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake
+++ /dev/null
@@ -1,57 +0,0 @@
-set(CMAKE_CXX_COMPILER "/usr/bin/c++")
-set(CMAKE_CXX_COMPILER_ARG1 "")
-set(CMAKE_CXX_COMPILER_ID "GNU")
-set(CMAKE_CXX_COMPILER_VERSION "4.8.2")
-set(CMAKE_CXX_PLATFORM_ID "Linux")
-
-set(CMAKE_AR "/usr/bin/ar")
-set(CMAKE_RANLIB "/usr/bin/ranlib")
-set(CMAKE_LINKER "/usr/bin/ld")
-set(CMAKE_COMPILER_IS_GNUCXX 1)
-set(CMAKE_CXX_COMPILER_LOADED 1)
-set(CMAKE_CXX_COMPILER_WORKS TRUE)
-set(CMAKE_CXX_ABI_COMPILED TRUE)
-set(CMAKE_COMPILER_IS_MINGW )
-set(CMAKE_COMPILER_IS_CYGWIN )
-if(CMAKE_COMPILER_IS_CYGWIN)
-  set(CYGWIN 1)
-  set(UNIX 1)
-endif()
-
-set(CMAKE_CXX_COMPILER_ENV_VAR "CXX")
-
-if(CMAKE_COMPILER_IS_MINGW)
-  set(MINGW 1)
-endif()
-set(CMAKE_CXX_COMPILER_ID_RUN 1)
-set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC)
-set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP)
-set(CMAKE_CXX_LINKER_PREFERENCE 30)
-set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1)
-
-# Save compiler ABI information.
-set(CMAKE_CXX_SIZEOF_DATA_PTR "8")
-set(CMAKE_CXX_COMPILER_ABI "ELF")
-set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-
-if(CMAKE_CXX_SIZEOF_DATA_PTR)
-  set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}")
-endif()
-
-if(CMAKE_CXX_COMPILER_ABI)
-  set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}")
-endif()
-
-if(CMAKE_CXX_LIBRARY_ARCHITECTURE)
-  set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-endif()
-
-
-
-
-set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;c")
-set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib")
-set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "")
-
-
-
diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_C.bin
deleted file mode 100755
index 2f2ebe478334290d81ee498f150d9bab78baa7e7..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 8547
zcmeHMeQaCR6+iaNheH#`?FyunQE#JLV9Fycp)(*x9M|b{>$F{)z_wy;o%kg&_@lF*
zOUufr$f#yP8T%0HDug=3ANhm*Ip{Vt(gjhZF{x}~LhB|Wx@>J}M-d&05Y3&=edoNq
zJUbH;5`XnqHP`pNbM86!<DC0(9*!sb{XQQs{P1<a+LKMaie&!<7+=a90KE`_7U+fR
zU?nsGK%=7B=p_j*XQ^o}1H`8RKm!0o{5B!tw>5EjE%9;57GjC|=D`-)<<l+p7cK$9
zihy!xYba=c4+VFsA6#~l9o<y299B`TljJ%{j>{8d7?<2X>>K_cr1l1>e_XD&&2UQO
zMQok1GWgq~hOB_yFD`qCk4u*O#*CFgJ3lEx#JG+0EjBI}sDE5i6>iF9hu8Jol*@GG
zvW3c?u04@;UF&-Ea#3HaY{K@Dcf;l(rHRL}gT`e!fLa6i{rvhPFK(P%(X%PF@1?gk
zKJ|y5ANiia^@06Cxn^9Cq5T2)!c|KG4GnESXn<b)hEt7t`r>b%yRRkom*1ZH@xIpE
zezxTKO{b>*`o~kxtQ|gi{;ltP^V<6!KD_VO2j362E`R5lqaBS$-~ID@G@G^Pa%O%Z
zmH@!Ag(QW-BK*Hxgnn!h`nOeIIv^6V*CiuO^iB``X+>WFy&n3AYQLj~{~-Wa3hmHK
z>xtLR<Eme6Fx70YGm{jL5uM`z;}@jYYbVa@GH8Q`sGZwM;3-940uhQQjL#E)H`zmO
z0tvEnE77kcfv+h3AoLL3K`uKyg3E3l2!X#v*-Dozkxyp}Oc^Ox#AvpV1(6sW5E-*%
zj%CZ1SsEOO<%$J!Fg=_zAzK(NigNZrvp6a;yQ~rjGn2N`AOse6t02T!zE~g?0z_hq
zC|j9qK~&0S#;q<}$XX!G0uqPIWftGKTWllcv0S=bHp{k6buHSEO!URXT79j4i``ou
zBj2L`(C6@<?H7p3HQ5&QqLVaWv+ef!LjOS@V|36uLSJJ&h_H+9<~JtJ1&qfUf7KEO
z<IyRr@$}6AU`)IcnC2F3$XlLm34>hyTuT_*SM`EijrDQ-c;;;>rCfa&>uB<s6H>}G
z=M&X8?lr3K7!&`vuyrtTd<s{Har_uAP~-S<TwkYOlu}+CZC|w>;OL#9oiJo{;d(SC
zW`jm`<^g|d`Y0+u!!D*?#!vF?_Er0lb`)#Oe{5DWsxwCQ_r|facN)GE#v8L0e`@+S
zw(y^<iz%b}sxAKx$?w0z*AfP(EFChcGpD~NrA)or`0qd%d>>3>|BQ({UTX>iSl2lw
zEFXF~9NY;;bw-{@o%ybmvV9etDN8A_jp{?^jB4fW<MEGC>tpeA#-a8X_T%zz|J{@?
z{xMTb7v<yBbo;9BBR@p%j#g##u0-|2=uo2iW^^!laCM|93`54jE-bevZ<twg(l~ZW
z!x=jrpOeyf^t^Q)%O|U6lhqH`S7lT#|C=!}?K5tDw{jLIaz`q9Pc#+1H!7w_M|I}^
zU>c|ST!5$Aw_vpZBsTYNxy|K9JGvt&o0uNX?uaM*Qvkk|+U?!6kDt3F<vswoc23G9
z#&jv=a{!Q(QoaHJFG(p|eW3OWgzv#^;M)`Ot!!-$Jn3r=;XZ<Agx4-fd86X6fpo+|
zet_$VfBce^XafX8{lSin?MogA?1wwAx&6kQum2*Fu}$2gQ*%<PSQZV24)|kjO}ks0
zacjnJ835QnC#8y+eZkNV{qbPO6IwhNKG+Zsc0S%{1iKG38NtXz^FXjS7mP%M-O*rY
zUohMk?C1-I`ho#v=kowCIVYvt?=IsqF5@yjYiy<STQ`*qKKFCkN+0-K*G6?dpRJ%e
z->+6uo$p82P`!ihQ+&_k@Bf*TMPxZd_a?scO*Po(RKDxX()TN<y?;@O=XC_0OA&mI
zWH@ITI^S^YReJ)=?>BfJLR?K{fX=Z@ztCvg!*;Vix`sOSIcfLj1o2C%^LSpM`Xqh-
ztibnJFLqNu4|vY)j195aZQ;(L;Yz`(gnRUezP4-4trb;WbAwF)(93)CR(crpvQ@In
zasHJpSY~M#=!K$X>e0SLmz5r)+E}5Y4_C6eOjkAosy3c3kAt4sTPW|%+oe^qzw9<k
z<!rIApdw13m&{xm1?bnVoCUf%lk1kb#{ykl2lP@=9r1N@T#S~|c~gvMO7k^aLZnNj
z^j=$nf8RG!Qf8#{*%8pkikA9gYq47#E|)<cDdzKL!QvtNR6zg1wZiL^-%0rWg#CxX
z>x%Iux<|7-zeDi*g#Ah~556BV)_CMC@;i$f#B-G!UU@t(eJEv-^|N0eA_Cuf+0WUZ
z7@Gl<PqRGx{VWk)qYh(Sz@IG7{&*MNcUYd|$P^jMaf9I<zk30c&$2xG=_DEaWfElj
znUBl+0aOg&I>&(%B*5eEwV&}Iy_>K+$DIo#&;4Zm-u~ll0lUib9LE9_NICAIJjQmf
z{1Y^;9CtbHM~J}joZ+4Ssz-h&#g&~N{S1De;_{~+d4898lH{NDOc={C4twNzpX#BG
za=d1E?LR{DJbwI+(o6Dq{dL2uf6^n*?|J;r$M1Ozul^T2@{{xqGD-R`vLJ!yk1>T;
z9oL<o_ho+f^YVM^uLD54OP=@b4th!R@_XxVk^Q{y;*A&){)F;>r}sdX=Y5OcW%>P;
z*C*@edHAPCKfi;_Vy9f|ox$>4zUz_aa|0{rWI+PUaf$PTOVqKS&K;d(Alu1cImSn1
zBg^yqTZ9C?{2q4zN&4B(87%LO-z?81KL4|BH|}Bm%t-K5Rh@mujnfpkx@qF@T@Y>L
z{%~AF|E#CKW0l?7CQsvD{r?LZ_?-RJ4GsLxKIw)AnzR48p+SSQueqT?qqASRp+S?g
z54oX1v$Marp+UgeH{8&m#n}(s(BKLu&by)2d5=YC(CWlxH#BH-;;kE6z4utv&ip&G
z8ybY1xaWojOPzS;h6c-=IOT>0pL61m8yYNk;))v@T<OFMH#E4)i34tEc-klbbVIB2
zu8z=Ph2xKIXf?Z{?FS9;KDy%q;GXNNV?W-b{P;faIfv^+zu=)qiSE6>-L2Z6x0C&2
zRMBh4^B}du_d&+b6@Tr#;A>~Se&N31gLc5@O9UrgP=7uCHx>U<IMibEGfpXb{rRoe
z&U;F5S?xOUp*<bY?U~;;MXy~aSE+%j?e7iL4##1}Es9>dem42+{D?Q-vBSri#)Wu5
z(QETKkNE67jyFH@6CZ9xwfUpNN^fo6<cOlz;$>=w_b0~di`aRR?BVkepIgpSf4zC7
zk5v1$c`Hf$e6C|O`LSQ~>z1+1Uq9b;wZHy&esvLgT(wi1pMsH+RkkXlqxuL4u`#wy
zBoo^Qg@F0=k+AY&1QSSQ;QWn?Bt)ht#&X5sbWUWfVyP_Bl|3+0%<syXmYLDl-TJk)
z^_f(z%D!p}vtX6>f_*v7RPy<~An2;9YAVmPfHgLdzTb?+iuqmHoLP$YCG-(HwUr*u
zf)M@Nq62Xe-@G2PSE^h58*j%>OPIFGl#60KUC8825bM9SIXaMtfpD{_L>mY(I1uAj
zHYB(7MU!GnfB*LQpcstyCF7_^<>fw67S5GxTQnhOr}DhXzTRGUzU)(H?YMLE$vZov
M_<v>a>@?ti0lyfbasU7T

diff --git a/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin b/build/CMakeFiles/2.8.12.2/CMakeDetermineCompilerABI_CXX.bin
deleted file mode 100755
index 16c737f2604406275900604b68676d59840e9fbb..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 8560
zcmeHMZ){w}5udx~e@E=QO9-t=()d~u)qwo4Lr4t~jnC(E-lgY+IyR(*%jWDm#~1F8
zd-o3PkU~^9sHYGbRYgss3Zbe}^MOiyqO?>csS!bls(}_nOOc`kS4jveh?H6t@m8~M
z=K1;ErJ_Qr`XRq4-rL!k+1a<dv%52oMC1Klj|UiD_$pv+S3{2?S$_e>mof)H4|IbM
zdSErIga!bpS2PPfB*EnzsphhY_%r~h1AuO?Md<cgJ#o2;__$;XvBdrsz!q9%)@QYF
z2@p~QltWvCeycs`-=%(V*-mzJQps{yMY(p8YbQA_Pmy6<^7wFU_<xA>4bk|x?6b_U
zOXNk|HEX8ucONP0YqG}0<$B`dlI6ZWYo^f7Pm3Ti4pM(t`Ys>P__(Ah+>*_VY*>Fw
zHr0{M<ja#ClieFSHmui61$~{e3H2lI=B>l3C;pDD^j($%s5OA?>sLPZ;2&=I&ZfP-
z=XbU~@$T5sWa2cg4;&B5H3Gol;Ql7~;&NY8U0uuf>YxYPaH_GN-sl?__xK`jK7aNH
z`<n0g(UPaXcJ}yRet-7Kbt7-T|JJv5tlIO)(S5%<^r63b`I#r*`{UnfCTpX^p83UC
z0szYvlN1tH;Qz@L=+9h%9#dm!h3=rWE*W<bz1>AWsOZ;2kBdI2^tV>=X8~X-1RzN3
ziPz0TYFsUFxY1f?Y3lfHqO%`h9HxHVcH+D)gBBPdJsU~jfTAyfAo&x<7l<Fo9@-}G
zWkvTx9no!MGb5w8yf%Ok_*<0BWYHA4WG2s)(NbBAW%3yiv7rHxN*B|+GbJ-!92$sZ
z3;Fa=awMAukxnJeBnW|pr38f7oh#%?qW}@xCQ4>1lNaSuI_1=!$!AOu={yogN+lNG
z@_-nmz9QLVsgy2RCe$=wb3E1?5$p7I`fX}(JaW$!{#meEt@Vy5ugLgtcf3OFa{+7J
z;J(1U!)T@TfqRI1LWCT2C%-X$vB@}C|Bf#N#)0Ez{kfX~z?goqX~xnhZ+qPr0=f22
zz7Pc3dO)tl`ZzY8KP{z{YhS@SZvFG8q?BFn$11PwG%9C|>32W4YbbW|IL^3n@&qn8
z<K#(PFXw(KrMxs2XzK%bZc}g<3>zJ|zKrQPzfqZe(3_Zf4m&`DE+t;TCV4v0wjXKF
zVU77u%xOkt)~Ni>IB{W<;W=fzHdpp0W`1o6ziwVi7?qbS`7<QH|IRL72%x-l*r?2&
zdss@Dc&UE1F9e>CW^jDQ^qq4JApmo=ZNl>57efABU{q%1sl@q*q?Ca+IA4-dqK?Y9
zFB+Bd>66h<u-Ajpi^h?_v-?9$JrMZqaZmIUrrhSBe3F<6w0#%(A$(7`BExsbDj$c3
zW0g0;L-9jv#~VU0Y#i#qa(n#7*{-*Z6PGodsdEGKQW^)|H&?TKymBF4`M9ql!*cmw
zjp-SWar=Aa3pj!ICc^iH6XBg<F*7!%Gyg|3IK3B}c#1uXRtZ3CYyY-8N}0Xs!k9=+
zn8kZLyAu{MIg+_I8t+d4cvfl~JLz1$cv;GQ0I+Ia$~gLHDdo=qATFhR5deN8rEK<q
z^8X0W-a+t820bgA8=DS$8iRN);F<sOWhrk`92Ss{Sj-QwhWN)XONlmsKiKbY-4a;x
zVAFosv|{5;x32jTl2Ip~xruoxm2V3BgO7S6Ee#JeH{wx?Z7BfQKQE>7fxZ6V_q|bn
z>*HF~A39VQ^|wD(Z}>YOZ7}@Z(~SfEo~*w+?C%Ww+k5??UVm$^KiKPUQg%KK0LSK~
zbndsSxQeT|ivM?n=>7&=68PTFWi!$FUe`i(zPDY^?etz+Np*f7t)O}<wevfV+y5~y
z3&?VW-c9`GJ631iQ~9kkNA1^;zBj4F`#J*erHJdOWY~8ex(BhZR%Zw-oKJXfLVTXe
z)Ad&WOn;f`Y&Z8u&rrKQFRk$$BYsJB{+=ID{b6eVUxDATZuHZA{chL2ov}F*xg*p*
zJW|e^<<NS)TVL1Fb$eM=yKb}y0D5UEXC_BLFPTNF9Oqw|yqPXefSxayX+7K<>oAkM
zskS>`)<?>jY^o!Z0#zGNmc~I(P322dIjb~_R?7qFVkuL|FRF+l=*4t4i30R%B5Q)K
zF7LXTo-{#M&jh_#P;UV}Jub$I$y{2Dr-}<TOF|@z#pIMF!N2#67L^&vTxJyX-33$q
zv3jvIj+9EEj}~&dbl&7o_L+eD2iFR(Q;w5x{Dk)pgVz<~26{)cJjWq8K4C>k7QpXA
zMwd(8q<v?SLcE7L;g-ky)yGm6xPRW)M~HAA3G+VZ{lwS`p!PJ&^S+-W!ZS2r^b7ct
z<#|8idlvUR%d;OjMuxKAV7R~EP5`xMS)TXl!(=dzpY3NpF7F3WK7i}&2ab^de}A|A
zjJ*^$VR`mDFOxivllym%A72+ZRF-EycA5m(?_m{vyIcNo`mXGE+3yF5!2X=!p8twV
zzLWe)C&_dF431B6`9qgH$0c@={7+pI#&V3KE_ptu>S&<suNiLppCWnwejG;$lKibK
zNO1Rm%q7q9JdX2mJdfe-|5=y(B*jN2ssBqXNZ|Qn9LJ}TBR0V2GROVg{O<az01$A<
z^SRwh(KI)|yZ#p0&*v__pb?=@DgO?|16iKWEso1_{FT=y_s{e2H&_20FPXzZIo3Oa
z<+*&%CC~Q;?x39o2`tAY&JQk8+kU!tw3C5sCxhh}{~{Y%p5t%bB<SXMZ3mFlzk8pt
zJo_&O%X5k0f9~6f`?!B*B)F=o&U?p+bL6<#4`5sn&l(;N`!(F3wfb*uv$XASDR=k(
zpP+%qK2M#{z-ynAPH3Rn=bsZA)Y<2n6B^Xp=amy0G}z~m6B;zy=Zg~>G}-5d6B_vJ
z^S}uWuCe{R6IzXUEJA~3+b=tzL5uBgozQB<X_cOZII|NP1Z}_Pga%7(f98Y+%WOa8
zga+5z{>KRomfL>C2@S5Z{e=@6e9ra*PH1@Br~T=KRwJ&C(BOJ|KRTgRor=~zsDlr2
zJ1zpAxt<#K<2%ZW@p;!hTqpW*7d=dL_xtT0rGLRr-XCL%Uj01}kskX!Ncgehubvl-
zj@BC2Da9WE41gl|9cKCC0ALst!ATjktCpURl%Az9?z8wAn7XX>-4M0(w5WkEt6D#+
zm7Z4Ubj|zCie9~b){`DSM;V(*5Bq1vc15pVR~e6$H*x1draYX1T#P3ay*j`1ln0NZ
z>ip5qmHz5{$}beX8n2QbKA#wWzJfjHJhk&m|D<ub^Gc0gw6i*I)$Fyt1K;Zy*Al<`
zd$tk3JKwa%Tl>4=JBjbuP7EqN)%htHEt(~>JT|6}f)HCGgCZW=J|qMzq?Lr36Qh_|
zDgh^Y93&x91+hC@7)fSD$}AL1B3Yh<(L!z_n>N!aeZ%cvSy!7$<*=-$rby?_;uKhq
z(^NT^n*u>kRaH|7sYR@jf#m(^$c`P6LT)0HO&7zxF@1Dm0>n4tT^lm_v4U^}^O4%E
zh{=%*2+=<n9*BzQ);>&vsX_B^d^_9yVD>9jDv0r9K7|>zzHe*|55yuMoNOvR2SN-D
zL|Dt___p3~Tx{#_-yR(jL*d?d6#G%xyib+8bEfPljc4Wi+!?{19%q*AGw1qvEDI?=
OEA9AS<^8PW;lBYd8IS4!

diff --git a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake b/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake
deleted file mode 100644
index 0616b7c1ee..0000000000
--- a/build/CMakeFiles/2.8.12.2/CMakeSystem.cmake
+++ /dev/null
@@ -1,15 +0,0 @@
-set(CMAKE_HOST_SYSTEM "Linux-3.13.0-24-generic")
-set(CMAKE_HOST_SYSTEM_NAME "Linux")
-set(CMAKE_HOST_SYSTEM_VERSION "3.13.0-24-generic")
-set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64")
-
-
-
-set(CMAKE_SYSTEM "Linux-3.13.0-24-generic")
-set(CMAKE_SYSTEM_NAME "Linux")
-set(CMAKE_SYSTEM_VERSION "3.13.0-24-generic")
-set(CMAKE_SYSTEM_PROCESSOR "x86_64")
-
-set(CMAKE_CROSSCOMPILING "FALSE")
-
-set(CMAKE_SYSTEM_LOADED 1)
diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c b/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c
deleted file mode 100644
index cba81d4a6b..0000000000
--- a/build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c
+++ /dev/null
@@ -1,389 +0,0 @@
-#ifdef __cplusplus
-# error "A C++ compiler has been selected for C."
-#endif
-
-/* Version number components: V=Version, R=Revision, P=Patch
-   Version date components:   YYYY=Year, MM=Month,   DD=Day  */
-
-#if defined(__18CXX)
-# define ID_VOID_MAIN
-#endif
-
-#if defined(__INTEL_COMPILER) || defined(__ICC)
-# define COMPILER_ID "Intel"
-  /* __INTEL_COMPILER = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
-# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER    % 10)
-# if defined(__INTEL_COMPILER_BUILD_DATE)
-  /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
-#  define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
-# endif
-
-#elif defined(__PATHCC__)
-# define COMPILER_ID "PathScale"
-# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
-# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
-# if defined(__PATHCC_PATCHLEVEL__)
-#  define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
-# endif
-
-#elif defined(__clang__)
-# define COMPILER_ID "Clang"
-# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
-# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
-# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
-
-#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
-# define COMPILER_ID "Embarcadero"
-# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
-# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
-# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__     & 0xFFFF)
-
-#elif defined(__BORLANDC__)
-# define COMPILER_ID "Borland"
-  /* __BORLANDC__ = 0xVRR */
-# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
-# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
-
-#elif defined(__WATCOMC__)
-# define COMPILER_ID "Watcom"
-  /* __WATCOMC__ = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
-# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100)
-
-#elif defined(__SUNPRO_C)
-# define COMPILER_ID "SunPro"
-# if __SUNPRO_C >= 0x5100
-   /* __SUNPRO_C = 0xVRRP */
-#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12)
-#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF)
-#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_C    & 0xF)
-# else
-   /* __SUNPRO_C = 0xVRP */
-#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8)
-#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF)
-#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_C    & 0xF)
-# endif
-
-#elif defined(__HP_cc)
-# define COMPILER_ID "HP"
-  /* __HP_cc = VVRRPP */
-# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000)
-# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100)
-# define COMPILER_VERSION_PATCH DEC(__HP_cc     % 100)
-
-#elif defined(__DECC)
-# define COMPILER_ID "Compaq"
-  /* __DECC_VER = VVRRTPPPP */
-# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000)
-# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000  % 100)
-# define COMPILER_VERSION_PATCH DEC(__DECC_VER         % 10000)
-
-#elif defined(__IBMC__)
-# if defined(__COMPILER_VER__)
-#  define COMPILER_ID "zOS"
-# else
-#  if __IBMC__ >= 800
-#   define COMPILER_ID "XL"
-#  else
-#   define COMPILER_ID "VisualAge"
-#  endif
-   /* __IBMC__ = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(__IBMC__/100)
-#  define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(__IBMC__    % 10)
-# endif
-
-#elif defined(__PGI)
-# define COMPILER_ID "PGI"
-# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
-# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
-# if defined(__PGIC_PATCHLEVEL__)
-#  define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
-# endif
-
-#elif defined(_CRAYC)
-# define COMPILER_ID "Cray"
-# define COMPILER_VERSION_MAJOR DEC(_RELEASE)
-# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
-
-#elif defined(__TI_COMPILER_VERSION__)
-# define COMPILER_ID "TI"
-  /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
-# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
-# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000   % 1000)
-# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__        % 1000)
-
-#elif defined(__TINYC__)
-# define COMPILER_ID "TinyCC"
-
-#elif defined(__SCO_VERSION__)
-# define COMPILER_ID "SCO"
-
-#elif defined(__GNUC__)
-# define COMPILER_ID "GNU"
-# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
-# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
-# if defined(__GNUC_PATCHLEVEL__)
-#  define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
-# endif
-
-#elif defined(_MSC_VER)
-# define COMPILER_ID "MSVC"
-  /* _MSC_VER = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
-# if defined(_MSC_FULL_VER)
-#  if _MSC_VER >= 1400
-    /* _MSC_FULL_VER = VVRRPPPPP */
-#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
-#  else
-    /* _MSC_FULL_VER = VVRRPPPP */
-#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
-#  endif
-# endif
-# if defined(_MSC_BUILD)
-#  define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
-# endif
-
-/* Analog VisualDSP++ >= 4.5.6 */
-#elif defined(__VISUALDSPVERSION__)
-# define COMPILER_ID "ADSP"
-  /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
-# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
-# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
-# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8  & 0xFF)
-
-/* Analog VisualDSP++ < 4.5.6 */
-#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
-# define COMPILER_ID "ADSP"
-
-/* IAR Systems compiler for embedded systems.
-   http://www.iar.com */
-#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC)
-# define COMPILER_ID "IAR"
-
-/* sdcc, the small devices C compiler for embedded systems,
-   http://sdcc.sourceforge.net  */
-#elif defined(SDCC)
-# define COMPILER_ID "SDCC"
-  /* SDCC = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(SDCC/100)
-#  define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(SDCC    % 10)
-
-#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION)
-# define COMPILER_ID "MIPSpro"
-# if defined(_SGI_COMPILER_VERSION)
-  /* _SGI_COMPILER_VERSION = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100)
-#  define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION    % 10)
-# else
-  /* _COMPILER_VERSION = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100)
-#  define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION    % 10)
-# endif
-
-/* This compiler is either not known or is too old to define an
-   identification macro.  Try to identify the platform and guess that
-   it is the native compiler.  */
-#elif defined(__sgi)
-# define COMPILER_ID "MIPSpro"
-
-#elif defined(__hpux) || defined(__hpua)
-# define COMPILER_ID "HP"
-
-#else /* unknown compiler */
-# define COMPILER_ID ""
-
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
-   getting matched.  Store it in a pointer rather than an array
-   because some compilers will just produce instructions to fill the
-   array rather than assigning a pointer to a static array.  */
-char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
-
-/* Identify known platforms by name.  */
-#if defined(__linux) || defined(__linux__) || defined(linux)
-# define PLATFORM_ID "Linux"
-
-#elif defined(__CYGWIN__)
-# define PLATFORM_ID "Cygwin"
-
-#elif defined(__MINGW32__)
-# define PLATFORM_ID "MinGW"
-
-#elif defined(__APPLE__)
-# define PLATFORM_ID "Darwin"
-
-#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
-# define PLATFORM_ID "Windows"
-
-#elif defined(__FreeBSD__) || defined(__FreeBSD)
-# define PLATFORM_ID "FreeBSD"
-
-#elif defined(__NetBSD__) || defined(__NetBSD)
-# define PLATFORM_ID "NetBSD"
-
-#elif defined(__OpenBSD__) || defined(__OPENBSD)
-# define PLATFORM_ID "OpenBSD"
-
-#elif defined(__sun) || defined(sun)
-# define PLATFORM_ID "SunOS"
-
-#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
-# define PLATFORM_ID "AIX"
-
-#elif defined(__sgi) || defined(__sgi__) || defined(_SGI)
-# define PLATFORM_ID "IRIX"
-
-#elif defined(__hpux) || defined(__hpux__)
-# define PLATFORM_ID "HP-UX"
-
-#elif defined(__HAIKU__)
-# define PLATFORM_ID "Haiku"
-
-#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
-# define PLATFORM_ID "BeOS"
-
-#elif defined(__QNX__) || defined(__QNXNTO__)
-# define PLATFORM_ID "QNX"
-
-#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
-# define PLATFORM_ID "Tru64"
-
-#elif defined(__riscos) || defined(__riscos__)
-# define PLATFORM_ID "RISCos"
-
-#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
-# define PLATFORM_ID "SINIX"
-
-#elif defined(__UNIX_SV__)
-# define PLATFORM_ID "UNIX_SV"
-
-#elif defined(__bsdos__)
-# define PLATFORM_ID "BSDOS"
-
-#elif defined(_MPRAS) || defined(MPRAS)
-# define PLATFORM_ID "MP-RAS"
-
-#elif defined(__osf) || defined(__osf__)
-# define PLATFORM_ID "OSF1"
-
-#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
-# define PLATFORM_ID "SCO_SV"
-
-#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
-# define PLATFORM_ID "ULTRIX"
-
-#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
-# define PLATFORM_ID "Xenix"
-
-#else /* unknown platform */
-# define PLATFORM_ID ""
-
-#endif
-
-/* For windows compilers MSVC and Intel we can determine
-   the architecture of the compiler being used.  This is because
-   the compilers do not have flags that can change the architecture,
-   but rather depend on which compiler is being used
-*/
-#if defined(_WIN32) && defined(_MSC_VER)
-# if defined(_M_IA64)
-#  define ARCHITECTURE_ID "IA64"
-
-# elif defined(_M_X64) || defined(_M_AMD64)
-#  define ARCHITECTURE_ID "x64"
-
-# elif defined(_M_IX86)
-#  define ARCHITECTURE_ID "X86"
-
-# elif defined(_M_ARM)
-#  define ARCHITECTURE_ID "ARM"
-
-# elif defined(_M_MIPS)
-#  define ARCHITECTURE_ID "MIPS"
-
-# elif defined(_M_SH)
-#  define ARCHITECTURE_ID "SHx"
-
-# else /* unknown architecture */
-#  define ARCHITECTURE_ID ""
-# endif
-
-#else
-#  define ARCHITECTURE_ID ""
-#endif
-
-/* Convert integer to decimal digit literals.  */
-#define DEC(n)                   \
-  ('0' + (((n) / 10000000)%10)), \
-  ('0' + (((n) / 1000000)%10)),  \
-  ('0' + (((n) / 100000)%10)),   \
-  ('0' + (((n) / 10000)%10)),    \
-  ('0' + (((n) / 1000)%10)),     \
-  ('0' + (((n) / 100)%10)),      \
-  ('0' + (((n) / 10)%10)),       \
-  ('0' +  ((n) % 10))
-
-/* Convert integer to hex digit literals.  */
-#define HEX(n)             \
-  ('0' + ((n)>>28 & 0xF)), \
-  ('0' + ((n)>>24 & 0xF)), \
-  ('0' + ((n)>>20 & 0xF)), \
-  ('0' + ((n)>>16 & 0xF)), \
-  ('0' + ((n)>>12 & 0xF)), \
-  ('0' + ((n)>>8  & 0xF)), \
-  ('0' + ((n)>>4  & 0xF)), \
-  ('0' + ((n)     & 0xF))
-
-/* Construct a string literal encoding the version number components. */
-#ifdef COMPILER_VERSION_MAJOR
-char const info_version[] = {
-  'I', 'N', 'F', 'O', ':',
-  'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
-  COMPILER_VERSION_MAJOR,
-# ifdef COMPILER_VERSION_MINOR
-  '.', COMPILER_VERSION_MINOR,
-#  ifdef COMPILER_VERSION_PATCH
-   '.', COMPILER_VERSION_PATCH,
-#   ifdef COMPILER_VERSION_TWEAK
-    '.', COMPILER_VERSION_TWEAK,
-#   endif
-#  endif
-# endif
-  ']','\0'};
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
-   getting matched.  Store it in a pointer rather than an array
-   because some compilers will just produce instructions to fill the
-   array rather than assigning a pointer to a static array.  */
-char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
-char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
-
-
-
-/*--------------------------------------------------------------------------*/
-
-#ifdef ID_VOID_MAIN
-void main() {}
-#else
-int main(int argc, char* argv[])
-{
-  int require = 0;
-  require += info_compiler[argc];
-  require += info_platform[argc];
-  require += info_arch[argc];
-#ifdef COMPILER_VERSION_MAJOR
-  require += info_version[argc];
-#endif
-  (void)argv;
-  return require;
-}
-#endif
diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out
deleted file mode 100755
index 33a3d2b580160e3c32c235a17c6d2c6dc266b182..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 8643
zcmeHMZ){sv6+iY%nsiBRr?il^uvlY7S+%^iSytRlqjp@U&y|ywG+8&b>5UURiJAWt
z`*}+SgF@IOE9+JXgtEbheL$i}d_<-2fwg2>KLAo%4QU9F%0#NRYo(%<DuOl_o%_yt
zd3kow;KM%U7scn@bMNoobN`+D@7u9NkJsY?h8Mm9DpVI%D(e^EWDBPN2tx?gLl_=}
zZLkgi>Xptym>4V<DQlLEluiSHIsgcHEk(#{mBjM%l#V4^h!V?N23u&!A2(PrECGT_
zfjqP|;IrZbzG3yjGE8<DB$<an<h2v8op>zg$uO2&Kdc-64^VysR6mycEHiA0X%UAO
z%oKi`Sg4X#y-bo_mfDug`|5(3BFZO1fKv5Sd8@U{ZK@wj3c_RA%*gJp$Fix8Y$jix
z?3fJg?%3U>mkN5PvI+CYwEO!8RY}~At<)}?0Mr=3`0u=RxufOa;FsR~)zp0H+TQP;
z?)%Zp$mRe41x*03u?FD_c8|J{&5Vqq*}Fjq{1zoMSu{m1naP6?<GDhfGy?V~;@wfv
zsdws60H_+Fjra$4dzQCm)??hGRLr8^8fVxy_Rs==)_|qQ;fj1l(4lvxH>Phk8Z-5=
z<{%iaoHy&QJq`fI^t+97%(Nh%=xh#x+}_n31b<r?<aP{CVC3~HQcAgf2Zpi8>+@2|
zUAN+uOUI4MhsN~ZZyy?nUpzlw9|q&%g<&Nw-l_}3wO>jp?~M7|_5qwb5*UU-qvKFu
z7>wyfpHW$O(K|GE4hulT?hL(yQSyqv?Ih~XVTjXTSk#Qlf>HUcapA^Z!!vJOS}c2q
z=6-FFFPnFUjLLf!|3l)ReEMfiL4b10pix=4_AM#p(0lbiYzl(s<2kIKG5z$j>w^I1
zgSH8q2Hy$#hQX*T$oZk`-;h%J+u(XhN{M+?UcPBm%2zJN?x|8|Hg`4!VJ3Fdm^D1}
z{<Gl!&3RAko-woep;h!7Yw1-6{<9~8>%x@7Ozhs!+}P>Jkw`^Go{3lPMh4@RKSl-;
zr?$^F24T=R)q(PG;*o`2e>N`M*RY4LZC;Ync;%M)pug>VXdfghHxiY*`zkUbH~qz!
zp7R)cJ}Te9jz2mSITjg;9FK^(u`!*~e>{i7;$|Zc4bO^ERE>CF&%q~03%L`SY`S=K
zf8XHH$}}tD#S7_TDO1QF-D#mqr;iYSbPidJ6WOFWRw(9<CeS;OGFcp*I64I2*`__U
zll=Ix`%-=r0A9N<WdZ=EmZba%0Nh-X@?8KpCZ%llfbtgz&rAK_nGATgH8(V#_A~_0
zkKh^+x-aDphv}$CtV|E^Fs1+Co|I?<_yRq?)&u^g7aLE)-mOpVc>LjqP>p$_kN(4b
zDZh@2h%fM}H@ab+H{0CMXvL+_rz}aC#`tbu;0<rg*ZR5^^94`U#eD6r)f>K@udXwE
zq3MQRUpVUvMSMFWzV>cku-n(#?F)4K8kL<t$GNj4rE|WoVGV0o!y4A`Y2rMc``#wW
z;5{nKW{NnYS?7}t6n?+n3U8q>pRcx2n9oOBDcnluDL&_M{6Cju0h7$qxye5BQF(mU
zIY;piP<|njcn(0|c>}STBm>W%3bfzmwO8#4V0phm$4U4cC7-17SwBo+wwudaPw6?l
zB(3^{DZQjHx94pNhbaEj0-s~uxJq{1UCkakkM@%sC%HcweKOcSI8x4=<zSZ{(mOkL
z?J29^u172dfL@x)naL5*OJ>oM6a1CQo9W^S(DMZ|tw*}!9cFTzLgV?eK2pwPQyrNU
zsL({RGy!^QDqot)S<)<8F|<)#5fDYti|K3<3G{U$Yl5E1XH3w|^rQ*;SSFtVy;w*k
z%_QjQ2{Beo=F(y!Ra_2P43R7rlT#LhzrQwGRAwY|nNiTk3#R(9O0jYrDV0DUE#z|P
zyy<T8{|h*;a18PI<@*!9SK)cZ;PJ-zB7hpp%;)<FzPGS$PnN;=4UAnbzDe`ZqzrMt
z?Sz|;d+fVX7Px$#@3W*}`+5HJJY)0$sJYI3_6O%kVNgQ!7g%@Z^Spe9?je}Z{)HU`
z`xAz{{f+~uxy*c?zaerk?5Eg%PRH_j0OcDv%>E%vd~ScY{fw9BK85-0kA{iQ_2ly1
z^~X~t_6hUZzfBUK{UP$v=ezl@Q@e6Kc|ABoeD?DUcmG#h{CV;>^RDt4d~e0_2QEI}
zTXI2{T^+_e##=5vuV3#|gRviHxb1(N_}qS1X#KlF{O>c7!2QWM<Kpu@AK(A+eILVJ
z{#h6Q4Ef<R)FF46NZ|fsoW~<D%>e|j*L)x7PVWx?9sv9fKCkDkbYJRD?+$-J_EUEQ
z-rOL9pOF7ox}Rk}uV;K;%lEQ8KDm4z2UlI?^L^bSR?0En8O&$-k&Dm!2`-?Wi3H}c
z#Qwn{YTHlyk#;h0H9q5CWFzzOx(#uN_*@RdH61`w`8>}V%y;{5=Cj0S0WRAKlgsCX
z1XoandG4$LZ6JrK<M95r!_gn?*KmH;Du1}m%5Bt@xx4)T1`Ry+dg_D*UVEK%LIcfS
z|D4dE&R*A?(4gL4ubj|uwAR>nY0zM=FHUICXs;ViXt3U151i280o%_zp+S@Fzn##a
z+4jp$Xt?Wlof8lm_-sGwga&@wKRTg7!1jAiXwYK&Gbc3IX!|KAH293|f1J=@lkHcW
z&|tIeFPzZev$h{_Lc>!4%`Yc3Xa%0PPH3>jo{vswRjZ;k59;6_I2~62*IZ8x`*C`B
zVFT=RwZBgKVV6EadiS~Rh{}K2PM#lQN?+ZcrzjsjCo<kt>8tw%Z{=&%3(Kjc|DDRG
z1!fzpb0p(WN?*GlYvr?`h#RZM!(Ek6b-!B;Uv2+J54L}GzqfnvooNH$YYBqgOvuA8
zhbu9l^wr};cyOs`tr{m|D*x(vlvVm_%uqh=aX7n*JwNpTw0K)#r)xa_0`-o0%KMEu
zrEi5WrRR0!614+97a`PLH~{=j*|P<nb>)AR(!2e}Ey_RS%KzV#53g&C2Cvor_&$YD
zcl5YN_113xpx3IeJD*2aN#CRLul9!t%m@5{4?T!Z^$YNv(zk-)vL8l^X2~p%jp?Hx
z#DQqPNW>2h2mz^dF+H9sndzc1b7C}G$frwCLnTD2AjY$Wkz`h+%tEmwlI2Oj7n^L_
zOsDkSd%oORJCVS{taUUM>AYE-0_!lFD(7-jAn0hTLITggE0RWglh3E4QF<7Pr<D0U
z{gK|7i1qCgf-1w`EBGl8`=0BI^v0tgoH6QU6~z9;!R|;x9PH^i92*b=k?uqcvr~_+
zqouMk8$^7dS1h;DKyMUVXdpR~O#>dKIkU7nR};tmfEx}wpKOJ25?4O$3Xv)m#6&Wm
z%BDfY4~mkR%H&14lukh=KUNU@pyf1FT^xus*4L_<O39CDAjC+iL^0NL@_`pbKXrOE
in=F;mB@kkuH_FY3b><<kqQ+KHDto@!Pyu{VgZ}`XxnsTn

diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
deleted file mode 100644
index e8220b26e5..0000000000
--- a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
+++ /dev/null
@@ -1,377 +0,0 @@
-/* This source file must have a .cpp extension so that all C++ compilers
-   recognize the extension without flags.  Borland does not know .cxx for
-   example.  */
-#ifndef __cplusplus
-# error "A C compiler has been selected for C++."
-#endif
-
-/* Version number components: V=Version, R=Revision, P=Patch
-   Version date components:   YYYY=Year, MM=Month,   DD=Day  */
-
-#if defined(__COMO__)
-# define COMPILER_ID "Comeau"
-  /* __COMO_VERSION__ = VRR */
-# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100)
-# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100)
-
-#elif defined(__INTEL_COMPILER) || defined(__ICC)
-# define COMPILER_ID "Intel"
-  /* __INTEL_COMPILER = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
-# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER    % 10)
-# if defined(__INTEL_COMPILER_BUILD_DATE)
-  /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
-#  define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
-# endif
-
-#elif defined(__PATHCC__)
-# define COMPILER_ID "PathScale"
-# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
-# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
-# if defined(__PATHCC_PATCHLEVEL__)
-#  define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
-# endif
-
-#elif defined(__clang__)
-# define COMPILER_ID "Clang"
-# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
-# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
-# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
-
-#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
-# define COMPILER_ID "Embarcadero"
-# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
-# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
-# define COMPILER_VERSION_PATCH HEX(__CODEGEARC_VERSION__     & 0xFFFF)
-
-#elif defined(__BORLANDC__)
-# define COMPILER_ID "Borland"
-  /* __BORLANDC__ = 0xVRR */
-# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
-# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
-
-#elif defined(__WATCOMC__)
-# define COMPILER_ID "Watcom"
-  /* __WATCOMC__ = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
-# define COMPILER_VERSION_MINOR DEC(__WATCOMC__ % 100)
-
-#elif defined(__SUNPRO_CC)
-# define COMPILER_ID "SunPro"
-# if __SUNPRO_CC >= 0x5100
-   /* __SUNPRO_CC = 0xVRRP */
-#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12)
-#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF)
-#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC    & 0xF)
-# else
-   /* __SUNPRO_CC = 0xVRP */
-#  define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8)
-#  define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF)
-#  define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC    & 0xF)
-# endif
-
-#elif defined(__HP_aCC)
-# define COMPILER_ID "HP"
-  /* __HP_aCC = VVRRPP */
-# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000)
-# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100)
-# define COMPILER_VERSION_PATCH DEC(__HP_aCC     % 100)
-
-#elif defined(__DECCXX)
-# define COMPILER_ID "Compaq"
-  /* __DECCXX_VER = VVRRTPPPP */
-# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000)
-# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000  % 100)
-# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER         % 10000)
-
-#elif defined(__IBMCPP__)
-# if defined(__COMPILER_VER__)
-#  define COMPILER_ID "zOS"
-# else
-#  if __IBMCPP__ >= 800
-#   define COMPILER_ID "XL"
-#  else
-#   define COMPILER_ID "VisualAge"
-#  endif
-   /* __IBMCPP__ = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
-#  define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(__IBMCPP__    % 10)
-# endif
-
-#elif defined(__PGI)
-# define COMPILER_ID "PGI"
-# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
-# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
-# if defined(__PGIC_PATCHLEVEL__)
-#  define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
-# endif
-
-#elif defined(_CRAYC)
-# define COMPILER_ID "Cray"
-# define COMPILER_VERSION_MAJOR DEC(_RELEASE)
-# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
-
-#elif defined(__TI_COMPILER_VERSION__)
-# define COMPILER_ID "TI"
-  /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
-# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
-# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000   % 1000)
-# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__        % 1000)
-
-#elif defined(__SCO_VERSION__)
-# define COMPILER_ID "SCO"
-
-#elif defined(__GNUC__)
-# define COMPILER_ID "GNU"
-# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
-# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
-# if defined(__GNUC_PATCHLEVEL__)
-#  define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
-# endif
-
-#elif defined(_MSC_VER)
-# define COMPILER_ID "MSVC"
-  /* _MSC_VER = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
-# if defined(_MSC_FULL_VER)
-#  if _MSC_VER >= 1400
-    /* _MSC_FULL_VER = VVRRPPPPP */
-#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
-#  else
-    /* _MSC_FULL_VER = VVRRPPPP */
-#   define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
-#  endif
-# endif
-# if defined(_MSC_BUILD)
-#  define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
-# endif
-
-/* Analog VisualDSP++ >= 4.5.6 */
-#elif defined(__VISUALDSPVERSION__)
-# define COMPILER_ID "ADSP"
-  /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
-# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
-# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
-# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8  & 0xFF)
-
-/* Analog VisualDSP++ < 4.5.6 */
-#elif defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
-# define COMPILER_ID "ADSP"
-
-/* IAR Systems compiler for embedded systems.
-   http://www.iar.com */
-#elif defined(__IAR_SYSTEMS_ICC__ ) || defined(__IAR_SYSTEMS_ICC)
-# define COMPILER_ID "IAR"
-
-#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION)
-# define COMPILER_ID "MIPSpro"
-# if defined(_SGI_COMPILER_VERSION)
-  /* _SGI_COMPILER_VERSION = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100)
-#  define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION    % 10)
-# else
-  /* _COMPILER_VERSION = VRP */
-#  define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100)
-#  define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10)
-#  define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION    % 10)
-# endif
-
-/* This compiler is either not known or is too old to define an
-   identification macro.  Try to identify the platform and guess that
-   it is the native compiler.  */
-#elif defined(__sgi)
-# define COMPILER_ID "MIPSpro"
-
-#elif defined(__hpux) || defined(__hpua)
-# define COMPILER_ID "HP"
-
-#else /* unknown compiler */
-# define COMPILER_ID ""
-
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
-   getting matched.  Store it in a pointer rather than an array
-   because some compilers will just produce instructions to fill the
-   array rather than assigning a pointer to a static array.  */
-char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
-
-/* Identify known platforms by name.  */
-#if defined(__linux) || defined(__linux__) || defined(linux)
-# define PLATFORM_ID "Linux"
-
-#elif defined(__CYGWIN__)
-# define PLATFORM_ID "Cygwin"
-
-#elif defined(__MINGW32__)
-# define PLATFORM_ID "MinGW"
-
-#elif defined(__APPLE__)
-# define PLATFORM_ID "Darwin"
-
-#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
-# define PLATFORM_ID "Windows"
-
-#elif defined(__FreeBSD__) || defined(__FreeBSD)
-# define PLATFORM_ID "FreeBSD"
-
-#elif defined(__NetBSD__) || defined(__NetBSD)
-# define PLATFORM_ID "NetBSD"
-
-#elif defined(__OpenBSD__) || defined(__OPENBSD)
-# define PLATFORM_ID "OpenBSD"
-
-#elif defined(__sun) || defined(sun)
-# define PLATFORM_ID "SunOS"
-
-#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
-# define PLATFORM_ID "AIX"
-
-#elif defined(__sgi) || defined(__sgi__) || defined(_SGI)
-# define PLATFORM_ID "IRIX"
-
-#elif defined(__hpux) || defined(__hpux__)
-# define PLATFORM_ID "HP-UX"
-
-#elif defined(__HAIKU__)
-# define PLATFORM_ID "Haiku"
-
-#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
-# define PLATFORM_ID "BeOS"
-
-#elif defined(__QNX__) || defined(__QNXNTO__)
-# define PLATFORM_ID "QNX"
-
-#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
-# define PLATFORM_ID "Tru64"
-
-#elif defined(__riscos) || defined(__riscos__)
-# define PLATFORM_ID "RISCos"
-
-#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
-# define PLATFORM_ID "SINIX"
-
-#elif defined(__UNIX_SV__)
-# define PLATFORM_ID "UNIX_SV"
-
-#elif defined(__bsdos__)
-# define PLATFORM_ID "BSDOS"
-
-#elif defined(_MPRAS) || defined(MPRAS)
-# define PLATFORM_ID "MP-RAS"
-
-#elif defined(__osf) || defined(__osf__)
-# define PLATFORM_ID "OSF1"
-
-#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
-# define PLATFORM_ID "SCO_SV"
-
-#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
-# define PLATFORM_ID "ULTRIX"
-
-#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
-# define PLATFORM_ID "Xenix"
-
-#else /* unknown platform */
-# define PLATFORM_ID ""
-
-#endif
-
-/* For windows compilers MSVC and Intel we can determine
-   the architecture of the compiler being used.  This is because
-   the compilers do not have flags that can change the architecture,
-   but rather depend on which compiler is being used
-*/
-#if defined(_WIN32) && defined(_MSC_VER)
-# if defined(_M_IA64)
-#  define ARCHITECTURE_ID "IA64"
-
-# elif defined(_M_X64) || defined(_M_AMD64)
-#  define ARCHITECTURE_ID "x64"
-
-# elif defined(_M_IX86)
-#  define ARCHITECTURE_ID "X86"
-
-# elif defined(_M_ARM)
-#  define ARCHITECTURE_ID "ARM"
-
-# elif defined(_M_MIPS)
-#  define ARCHITECTURE_ID "MIPS"
-
-# elif defined(_M_SH)
-#  define ARCHITECTURE_ID "SHx"
-
-# else /* unknown architecture */
-#  define ARCHITECTURE_ID ""
-# endif
-
-#else
-#  define ARCHITECTURE_ID ""
-#endif
-
-/* Convert integer to decimal digit literals.  */
-#define DEC(n)                   \
-  ('0' + (((n) / 10000000)%10)), \
-  ('0' + (((n) / 1000000)%10)),  \
-  ('0' + (((n) / 100000)%10)),   \
-  ('0' + (((n) / 10000)%10)),    \
-  ('0' + (((n) / 1000)%10)),     \
-  ('0' + (((n) / 100)%10)),      \
-  ('0' + (((n) / 10)%10)),       \
-  ('0' +  ((n) % 10))
-
-/* Convert integer to hex digit literals.  */
-#define HEX(n)             \
-  ('0' + ((n)>>28 & 0xF)), \
-  ('0' + ((n)>>24 & 0xF)), \
-  ('0' + ((n)>>20 & 0xF)), \
-  ('0' + ((n)>>16 & 0xF)), \
-  ('0' + ((n)>>12 & 0xF)), \
-  ('0' + ((n)>>8  & 0xF)), \
-  ('0' + ((n)>>4  & 0xF)), \
-  ('0' + ((n)     & 0xF))
-
-/* Construct a string literal encoding the version number components. */
-#ifdef COMPILER_VERSION_MAJOR
-char const info_version[] = {
-  'I', 'N', 'F', 'O', ':',
-  'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
-  COMPILER_VERSION_MAJOR,
-# ifdef COMPILER_VERSION_MINOR
-  '.', COMPILER_VERSION_MINOR,
-#  ifdef COMPILER_VERSION_PATCH
-   '.', COMPILER_VERSION_PATCH,
-#   ifdef COMPILER_VERSION_TWEAK
-    '.', COMPILER_VERSION_TWEAK,
-#   endif
-#  endif
-# endif
-  ']','\0'};
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
-   getting matched.  Store it in a pointer rather than an array
-   because some compilers will just produce instructions to fill the
-   array rather than assigning a pointer to a static array.  */
-char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
-char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
-
-
-
-/*--------------------------------------------------------------------------*/
-
-int main(int argc, char* argv[])
-{
-  int require = 0;
-  require += info_compiler[argc];
-  require += info_platform[argc];
-#ifdef COMPILER_VERSION_MAJOR
-  require += info_version[argc];
-#endif
-  (void)argv;
-  return require;
-}
diff --git a/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out b/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out
deleted file mode 100755
index db35dbde0e2fe3ccc9c963b6592aa3f3f3eb7206..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 8652
zcmeHMZ){sv6+iY%nsiBRw`(D7S!0zIWfgf#vMji(*4as&zEn<H+GOo$(;Fvt5^w$!
z+0R?D4X6m4FkQ!l5Gn|XA@LzdfW((lkYMRjtrCAkhJ-Z4kh+PCwrfSvN{OJ&Md!YA
zUS6JEH2APj`BlyFz4!d?IrrbW@80{xXsp}o@c_dMdqAbC!Ya%93vhBhYXF2H1ZyD-
z+h8lK0f1_iXCX`smWxz1%UaUY0H6v0LS8E)<h3lZ`~>N-WDilIy=Ab6mi$hQRl*V=
zs4|d;z6N|&dB8WMey|La9|lS0VG?;w#A_lR%XxB)CASaThX4DizJ6*S%e|HxwnSaT
z!38sczfCl%q}48yWV@AZOXhuc!Aub4Lm@y~hsfUD`sD_-k0mAH!E|c4qy52jqB)()
z7AKo0LmkZ>?Rp`nw<({ne$?I9H=rzWKQ>UmYy?nq0L%T~ZTiWcUqAPwue>7PY2JPP
zXE#4J@>S&W|NjMb0I;C~;XZprT}Y>fN6_sK5CVUTf*H@7A`?$#L5Q(TE=xHA_QiTS
zyF{DbraugzT7*90Kd}3=xFxj~%RWtJ7X8*d!?Cfa763E^tbClV$Y%r{`A&Uf`dY1V
zsybR91mlHSv-<Kw0ANgCsGVb`1^IAWeGuf1_WB_B8^a)XV0s)2uUwK+${jl~jV7<m
zODS8g_mti|VU*rArvGx|V1LiW+1cta7#GhURN~_GsxVyssg&~OsK0S9z_}xVAs8^4
z4+e(7m|pZ5rG;m_gLCK506KPa@HH%wm;8+<G4C9vSpWQ@W|S6;((A_gtB)C;dE?E+
zqIYoa=N9?*=FLH)^oGTMm-r_i{Xtz2ptyd(C@oz6f|PRbjp}dK1;O*d9JbGxe)Q<t
zAb`2ec46bdYeC--7^MX{KX~OiDW$&=t`ww{SV!se*Njr}(#7a)W%bmiwz?pkie59$
z7@m3mS@8dI))T#LoZ7U#Qod63pFJ5~6DG}5(c6P_qq@KG%UB;ojzmf_@_0|_R%D>3
z^oK})?DUSA+8_)Vr<+k8jy<r@`X}T39SukH@{T1bjTf$)+n66KU5%A)?JdcO-1xRJ
zJ?AlYy<fbF1AS~Tay&8^IS~<aqs#U_n8Rsst+v8cQ1GmnMdj$}>pt+vNG>yxN+<Kj
z_Vo=6T3Hk6xH+23XO6{ic~fRQKQex7a7866_~PkgzL3ggkF{86*Xc)yf3zXnvsK&O
zLUH`~9Vwp&fS2w_83TZ+B`LoT0B<izc>w@oQp$P{sCa?!JbMT{lL61x`kLBTJT(D~
zBe+L|?nt@Q;Th&5R_X)XPx^1&mJ)pcU!dF9u-{+zOzlZ{Y|HMQ58b~Vv$0N$(Z9VT
z<rA0@@daM=cCB0E?XRz?waOB>?w6!YVtJ=8@HKDL*YL6y^#xB?MSV>#RU5vR7uOiR
z&~#0&FP!#;BEFW0uc^})?DREs`U0K4TIJ^t(f=hWo$GxSt60S<R<VkY60>yfdzB=E
zzfoD%Q^Aa8oln+L`a-pp-b`seUu~r{pO3asx`ED9e9q(Ye=Nxys+^&7lYQnRdwkY8
zN9FfWeIb%~4nW{}1F?xD1J9od^u5h{ulgo{<?jtTPQu4YeUj{R{t%_vZ??CV^f|pG
zt@eaTUs9U;^9H3uRQ^$c&#`X2M}FM8n?H0Oy_aMY$$ed2j|7_rhKpIV7;M)=dRuer
zuA)k|K44`4=!L0_86O6{VCF43&c9MwGnt<NJ)1L=dZe?b*^G}-YAjpShl{CnqB)fS
zl^Ty1#z9X^WeZaoOPYDBgg!)9Bt#zcd@>zJ0{xmuo1mw%DHC)vIcb7En#!g?&*u_x
zGY)!kT#V-9nWPv`<d;(xL&Wp>_>{%q-%pO@l^gL)Y6SGLoT>g;R;(I_3kA?ea+ypr
zYr4Dq{{pTnoI^Z+`Tm6ORd`)7c)l?{383aO^Z9;)?=7s`lV$LI1EbZ&H)%bZR3UzE
zJK^TzH})+lb8Mg2`x(k$|9SoMI%D(!sI|^~jtA!`!yrYB7ua^@^SXST?je}Z@r45f
z#}kIT|4sm?wak28zaa`R9H-cS)?@h;fQk*A=J*gMKKH-df5x+PpTc~OM?=KtcCvkU
z`|;F@W5RrnZ<EC5c!+$A`ELHp)UVu5-VbJo&vBmN9{-YyKTq*y-esS`_f{;w;o|eX
zB^!F(HDJtReA~t6{p$jC7{_sj+y7UI&;56a_P<NS|2h*1Jf4gh7oYF>`2LUY`xtKf
zXI=amio-KBAUByv;PGS3;t`nE0D|{xz7KTkyVJh`0KbFJ`*{Q1m%8=c>9@##8g9Uw
z8$|F!^8ZZtv&`rHjPGmtUY6%4+vj=kp36Sp*DYeB9P^#Qe3tLK`20P=2AY^iU>-{x
zADp7L|MWf5L=N7K&-fSl$b7tRLmVVN+hMqt14y#Z>zu)Scl>5POMDh!+fJBlpA`wN
zq)PMJSpoWh9HN24`&->H9vs(jeO9tR)M(X~c2(}S|KFg2$KFq!(7<c&lTK)$+54Xp
z8dTZ)niCpS+xwLh8qU@V-(4Ei*!znU8r0hRh7%gBwf6%jG`Po(^G;|`XUA_RG^n@Z
zvJ)D9^}Egq2n~F89Cbniza1Z)&>&#PJts6+Z^ttyG}vIrDJL|z*N#6<Xt2?aD^6&z
z$&MFJXz(#R4mhFVDS+0O6B;xCuUjWH*le#yC$zF%(OL&p@ONB}D}Z~hr-J{uJiM?D
zT3r2)YXtL$T=@~occ1HysQQ=v<n=MC^2_`4G}XiBM8>z2e)+iIt$d|+{Z8ro0WZ=K
zZB!B3ucZHu(q9i}YAk)m-&B6(@vT(PzZG#q**w8JW>1Crut{}DdApjFe))XZ<H7f*
zb%3uZ2o67e9)3|=i6>Ql`TQw*a0_WDn?KL1`pehXmsEZ^zDM=A=jV^_;?FNU0PDRC
z(BhiszruXS`sMGCKdSr&2$MeVH-Dvmz-J_cx+4dGe=2`A!%<iLw@Kd}Pc$#uZGe!g
z{`Fq$pXGVWxX)|#KfYTb)ZIM*Z1+~~{|>C*Ti(x|chNtt>MxIz!{n#o@)Jh#X2C3u
zj_M;I#Qv^BBGz-bUkFGf^U1MP!A$0bnGqxDTsB#N3RyxVa$+o<8;++%!p!9hB3_&X
ze6dL<&16FF*!7vV%1Qzcv)0j6B(r9I3arCyqL|4{fuN(UN(npzuTbskjX#y_I(oE=
zo`!l7`pCoth|k4Z+fv!l96$Ad5Z#9&y-^YE+baY$k$<nKH34Gp6Md21o-Pp167}8+
zVqfe)XCx*Lbax+)_KW^VXDo`<sVCWyLQypaL{EROSnj$0-Y)F7{`hb@33$Nf%984|
zO&s^LZaD0G$Q8m#T>0QDM52%r<MC`FodnTyKorbGDl3YGWCBzt@GF_qQFWallGt9W
zX-boy*g%NkLV-%GN9O%ci$gROy3+ANAz1(+`g^;$8?ntiF;=wLGNr1=mpS+k*Z*B!

diff --git a/build/CMakeFiles/CMakeDirectoryInformation.cmake b/build/CMakeFiles/CMakeDirectoryInformation.cmake
deleted file mode 100644
index b2cc9b7e7d..0000000000
--- a/build/CMakeFiles/CMakeDirectoryInformation.cmake
+++ /dev/null
@@ -1,16 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# Relative path conversion top directories.
-SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/roman/src/Firmware/src")
-SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build")
-
-# Force unix paths in dependencies.
-SET(CMAKE_FORCE_UNIX_PATHS 1)
-
-
-# The C and CXX include file regular expressions for this directory.
-SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
-SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
-SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
-SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/build/CMakeFiles/CMakeError.log b/build/CMakeFiles/CMakeError.log
deleted file mode 100644
index 9ab6f1e9a6..0000000000
--- a/build/CMakeFiles/CMakeError.log
+++ /dev/null
@@ -1,53 +0,0 @@
-Determining if the pthread_create exist failed with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec1424100880/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec1424100880.dir/build.make CMakeFiles/cmTryCompileExec1424100880.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building C object CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o
-/usr/bin/cc    -o CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o   -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c
-Linking C executable cmTryCompileExec1424100880
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec1424100880.dir/link.txt --verbose=1
-/usr/bin/cc       CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o  -o cmTryCompileExec1424100880 -rdynamic 
-CMakeFiles/cmTryCompileExec1424100880.dir/CheckSymbolExists.c.o: In function `main':
-CheckSymbolExists.c:(.text+0x16): undefined reference to `pthread_create'
-collect2: error: ld returned 1 exit status
-make[1]: *** [cmTryCompileExec1424100880] Error 1
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-make: *** [cmTryCompileExec1424100880/fast] Error 2
-
-File /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c:
-/* */
-#include <pthread.h>
-
-int main(int argc, char** argv)
-{
-  (void)argv;
-#ifndef pthread_create
-  return ((int*)(&pthread_create))[argc];
-#else
-  (void)argc;
-  return 0;
-#endif
-}
-
-Determining if the function pthread_create exists in the pthreads failed with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec26988121/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec26988121.dir/build.make CMakeFiles/cmTryCompileExec26988121.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building C object CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o
-/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create   -o CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o   -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c
-Linking C executable cmTryCompileExec26988121
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec26988121.dir/link.txt --verbose=1
-/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create    CMakeFiles/cmTryCompileExec26988121.dir/CheckFunctionExists.c.o  -o cmTryCompileExec26988121 -rdynamic -lpthreads 
-/usr/bin/ld: cannot find -lpthreads
-collect2: error: ld returned 1 exit status
-make[1]: *** [cmTryCompileExec26988121] Error 1
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-make: *** [cmTryCompileExec26988121/fast] Error 2
-
-
diff --git a/build/CMakeFiles/CMakeOutput.log b/build/CMakeFiles/CMakeOutput.log
deleted file mode 100644
index a3757fc345..0000000000
--- a/build/CMakeFiles/CMakeOutput.log
+++ /dev/null
@@ -1,293 +0,0 @@
-The system is: Linux - 3.13.0-24-generic - x86_64
-Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded.
-Compiler: /usr/bin/cc 
-Build flags: 
-Id flags: 
-
-The output was:
-0
-
-
-Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out"
-
-The C compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdC/a.out"
-
-Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded.
-Compiler: /usr/bin/c++ 
-Build flags: 
-Id flags: 
-
-The output was:
-0
-
-
-Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out"
-
-The CXX compiler identification is GNU, found in "/home/roman/src/Firmware/build/CMakeFiles/2.8.12.2/CompilerIdCXX/a.out"
-
-Determining if the C compiler works passed with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec4189733644/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec4189733644.dir/build.make CMakeFiles/cmTryCompileExec4189733644.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building C object CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o
-/usr/bin/cc    -o CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o   -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCCompiler.c
-Linking C executable cmTryCompileExec4189733644
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4189733644.dir/link.txt --verbose=1
-/usr/bin/cc       CMakeFiles/cmTryCompileExec4189733644.dir/testCCompiler.c.o  -o cmTryCompileExec4189733644 -rdynamic 
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-
-
-Detecting C compiler ABI info compiled with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o
-/usr/bin/cc    -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o   -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c
-Linking C executable cmTryCompileExec2944435992
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1
-/usr/bin/cc     -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o  -o cmTryCompileExec2944435992 -rdynamic  
-Using built-in specs.
-COLLECT_GCC=/usr/bin/cc
-COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper
-Target: x86_64-linux-gnu
-Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu
-Thread model: posix
-gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) 
-COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/
-LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/
-COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64'
- /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-
-
-Parsed C implicit link information from above output:
-  link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)]
-  ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp]
-  ignore line: []
-  ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec2944435992/fast"]
-  ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec2944435992.dir/build.make CMakeFiles/cmTryCompileExec2944435992.dir/build]
-  ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp']
-  ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1]
-  ignore line: [Building C object CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o]
-  ignore line: [/usr/bin/cc    -o CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o   -c /usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c]
-  ignore line: [Linking C executable cmTryCompileExec2944435992]
-  ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec2944435992.dir/link.txt --verbose=1]
-  ignore line: [/usr/bin/cc     -v CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o  -o cmTryCompileExec2944435992 -rdynamic  ]
-  ignore line: [Using built-in specs.]
-  ignore line: [COLLECT_GCC=/usr/bin/cc]
-  ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper]
-  ignore line: [Target: x86_64-linux-gnu]
-  ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu]
-  ignore line: [Thread model: posix]
-  ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ]
-  ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/]
-  ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/]
-  ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec2944435992' '-rdynamic' '-mtune=generic' '-march=x86-64']
-  link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec2944435992 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o -lgcc --as-needed -lgcc_s --no-as-needed -lc -lgcc --as-needed -lgcc_s --no-as-needed /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o]
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore
-    arg [--sysroot=/] ==> ignore
-    arg [--build-id] ==> ignore
-    arg [--eh-frame-hdr] ==> ignore
-    arg [-m] ==> ignore
-    arg [elf_x86_64] ==> ignore
-    arg [--hash-style=gnu] ==> ignore
-    arg [--as-needed] ==> ignore
-    arg [-export-dynamic] ==> ignore
-    arg [-dynamic-linker] ==> ignore
-    arg [/lib64/ld-linux-x86-64.so.2] ==> ignore
-    arg [-zrelro] ==> ignore
-    arg [-o] ==> ignore
-    arg [cmTryCompileExec2944435992] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8]
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu]
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib]
-    arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu]
-    arg [-L/lib/../lib] ==> dir [/lib/../lib]
-    arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu]
-    arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib]
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..]
-    arg [CMakeFiles/cmTryCompileExec2944435992.dir/CMakeCCompilerABI.c.o] ==> ignore
-    arg [-lgcc] ==> lib [gcc]
-    arg [--as-needed] ==> ignore
-    arg [-lgcc_s] ==> lib [gcc_s]
-    arg [--no-as-needed] ==> ignore
-    arg [-lc] ==> lib [c]
-    arg [-lgcc] ==> lib [gcc]
-    arg [--as-needed] ==> ignore
-    arg [-lgcc_s] ==> lib [gcc_s]
-    arg [--no-as-needed] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore
-  remove lib [gcc]
-  remove lib [gcc_s]
-  remove lib [gcc]
-  remove lib [gcc_s]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib]
-  collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu]
-  collapse library dir [/lib/../lib] ==> [/lib]
-  collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
-  collapse library dir [/usr/lib/../lib] ==> [/usr/lib]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib]
-  implicit libs: [c]
-  implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib]
-  implicit fwks: []
-
-
-Determining if the CXX compiler works passed with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec191173331/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec191173331.dir/build.make CMakeFiles/cmTryCompileExec191173331.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building CXX object CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o
-/usr/bin/c++     -o CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx
-Linking CXX executable cmTryCompileExec191173331
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec191173331.dir/link.txt --verbose=1
-/usr/bin/c++        CMakeFiles/cmTryCompileExec191173331.dir/testCXXCompiler.cxx.o  -o cmTryCompileExec191173331 -rdynamic 
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-
-
-Detecting CXX compiler ABI info compiled with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o
-/usr/bin/c++     -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp
-Linking CXX executable cmTryCompileExec843003076
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1
-/usr/bin/c++      -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o  -o cmTryCompileExec843003076 -rdynamic  
-Using built-in specs.
-COLLECT_GCC=/usr/bin/c++
-COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper
-Target: x86_64-linux-gnu
-Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu
-Thread model: posix
-gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) 
-COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/
-LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/
-COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64'
- /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-
-
-Parsed CXX implicit link information from above output:
-  link line regex: [^( *|.*[/\])(ld|([^/\]+-)?ld|collect2)[^/\]*( |$)]
-  ignore line: [Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp]
-  ignore line: []
-  ignore line: [Run Build Command:/usr/bin/make "cmTryCompileExec843003076/fast"]
-  ignore line: [/usr/bin/make -f CMakeFiles/cmTryCompileExec843003076.dir/build.make CMakeFiles/cmTryCompileExec843003076.dir/build]
-  ignore line: [make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp']
-  ignore line: [/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1]
-  ignore line: [Building CXX object CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o]
-  ignore line: [/usr/bin/c++     -o CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp]
-  ignore line: [Linking CXX executable cmTryCompileExec843003076]
-  ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec843003076.dir/link.txt --verbose=1]
-  ignore line: [/usr/bin/c++      -v CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o  -o cmTryCompileExec843003076 -rdynamic  ]
-  ignore line: [Using built-in specs.]
-  ignore line: [COLLECT_GCC=/usr/bin/c++]
-  ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/4.8/lto-wrapper]
-  ignore line: [Target: x86_64-linux-gnu]
-  ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 4.8.2-19ubuntu1' --with-bugurl=file:///usr/share/doc/gcc-4.8/README.Bugs --enable-languages=c,c++,java,go,d,fortran,objc,obj-c++ --prefix=/usr --program-suffix=-4.8 --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --with-gxx-include-dir=/usr/include/c++/4.8 --libdir=/usr/lib --enable-nls --with-sysroot=/ --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --enable-gnu-unique-object --disable-libmudflap --enable-plugin --with-system-zlib --disable-browser-plugin --enable-java-awt=gtk --enable-gtk-cairo --with-java-home=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64/jre --enable-java-home --with-jvm-root-dir=/usr/lib/jvm/java-1.5.0-gcj-4.8-amd64 --with-jvm-jar-dir=/usr/lib/jvm-exports/java-1.5.0-gcj-4.8-amd64 --with-arch-directory=amd64 --with-ecj-jar=/usr/share/java/eclipse-ecj.jar --enable-objc-gc --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --with-tune=generic --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu]
-  ignore line: [Thread model: posix]
-  ignore line: [gcc version 4.8.2 (Ubuntu 4.8.2-19ubuntu1) ]
-  ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/]
-  ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/4.8/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../:/lib/:/usr/lib/]
-  ignore line: [COLLECT_GCC_OPTIONS='-v' '-o' 'cmTryCompileExec843003076' '-rdynamic' '-shared-libgcc' '-mtune=generic' '-march=x86-64']
-  link line: [ /usr/lib/gcc/x86_64-linux-gnu/4.8/collect2 --sysroot=/ --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -z relro -o cmTryCompileExec843003076 /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o -L/usr/lib/gcc/x86_64-linux-gnu/4.8 -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../.. CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o /usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o]
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/collect2] ==> ignore
-    arg [--sysroot=/] ==> ignore
-    arg [--build-id] ==> ignore
-    arg [--eh-frame-hdr] ==> ignore
-    arg [-m] ==> ignore
-    arg [elf_x86_64] ==> ignore
-    arg [--hash-style=gnu] ==> ignore
-    arg [--as-needed] ==> ignore
-    arg [-export-dynamic] ==> ignore
-    arg [-dynamic-linker] ==> ignore
-    arg [/lib64/ld-linux-x86-64.so.2] ==> ignore
-    arg [-zrelro] ==> ignore
-    arg [-o] ==> ignore
-    arg [cmTryCompileExec843003076] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crt1.o] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crti.o] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtbegin.o] ==> ignore
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8]
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu]
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib]
-    arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu]
-    arg [-L/lib/../lib] ==> dir [/lib/../lib]
-    arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu]
-    arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib]
-    arg [-L/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..]
-    arg [CMakeFiles/cmTryCompileExec843003076.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore
-    arg [-lstdc++] ==> lib [stdc++]
-    arg [-lm] ==> lib [m]
-    arg [-lgcc_s] ==> lib [gcc_s]
-    arg [-lgcc] ==> lib [gcc]
-    arg [-lc] ==> lib [c]
-    arg [-lgcc_s] ==> lib [gcc_s]
-    arg [-lgcc] ==> lib [gcc]
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/crtend.o] ==> ignore
-    arg [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu/crtn.o] ==> ignore
-  remove lib [gcc_s]
-  remove lib [gcc]
-  remove lib [gcc_s]
-  remove lib [gcc]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8] ==> [/usr/lib/gcc/x86_64-linux-gnu/4.8]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../../../lib] ==> [/usr/lib]
-  collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu]
-  collapse library dir [/lib/../lib] ==> [/lib]
-  collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
-  collapse library dir [/usr/lib/../lib] ==> [/usr/lib]
-  collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/4.8/../../..] ==> [/usr/lib]
-  implicit libs: [stdc++;m;c]
-  implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/4.8;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib]
-  implicit fwks: []
-
-
-Determining if files pthread.h exist passed with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec3897003384/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec3897003384.dir/build.make CMakeFiles/cmTryCompileExec3897003384.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building C object CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o
-/usr/bin/cc    -o CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o   -c /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CheckIncludeFiles.c
-Linking C executable cmTryCompileExec3897003384
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec3897003384.dir/link.txt --verbose=1
-/usr/bin/cc       CMakeFiles/cmTryCompileExec3897003384.dir/CheckIncludeFiles.c.o  -o cmTryCompileExec3897003384 -rdynamic 
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-
-
-Determining if the function pthread_create exists in the pthread passed with the following output:
-Change Dir: /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp
-
-Run Build Command:/usr/bin/make "cmTryCompileExec4077710905/fast"
-/usr/bin/make -f CMakeFiles/cmTryCompileExec4077710905.dir/build.make CMakeFiles/cmTryCompileExec4077710905.dir/build
-make[1]: Entering directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-/usr/bin/cmake -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles/CMakeTmp/CMakeFiles 1
-Building C object CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o
-/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create   -o CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o   -c /usr/share/cmake-2.8/Modules/CheckFunctionExists.c
-Linking C executable cmTryCompileExec4077710905
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTryCompileExec4077710905.dir/link.txt --verbose=1
-/usr/bin/cc   -DCHECK_FUNCTION_EXISTS=pthread_create    CMakeFiles/cmTryCompileExec4077710905.dir/CheckFunctionExists.c.o  -o cmTryCompileExec4077710905 -rdynamic -lpthread 
-make[1]: Leaving directory `/home/roman/src/Firmware/build/CMakeFiles/CMakeTmp'
-
-
diff --git a/build/CMakeFiles/CMakeRuleHashes.txt b/build/CMakeFiles/CMakeRuleHashes.txt
deleted file mode 100644
index b1bfe30c0e..0000000000
--- a/build/CMakeFiles/CMakeRuleHashes.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-# Hashes of file build rules.
-353bd50eb4c4b757d9a3d734d52d4f76 CMakeFiles/clean_test_results
-d83f452b18a5909d95cdb786c10abffb CMakeFiles/doxygen
-d83f452b18a5909d95cdb786c10abffb CMakeFiles/run_tests
-d83f452b18a5909d95cdb786c10abffb CMakeFiles/tests
diff --git a/build/CMakeFiles/Makefile.cmake b/build/CMakeFiles/Makefile.cmake
deleted file mode 100644
index d232bacff9..0000000000
--- a/build/CMakeFiles/Makefile.cmake
+++ /dev/null
@@ -1,150 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# The generator used is:
-SET(CMAKE_DEPENDS_GENERATOR "Unix Makefiles")
-
-# The top level Makefile was generated from the following files:
-SET(CMAKE_MAKEFILE_DEPENDS
-  "CMakeCache.txt"
-  "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake"
-  "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake"
-  "CMakeFiles/2.8.12.2/CMakeSystem.cmake"
-  "catkin/catkin_generated/version/package.cmake"
-  "catkin_generated/order_packages.cmake"
-  "/home/roman/src/Firmware/src/CMakeLists.txt"
-  "/opt/ros/indigo/share/catkin/cmake/../package.xml"
-  "/opt/ros/indigo/share/catkin/cmake/all.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/assert.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/atomic_configure_file.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkinConfig-version.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_add_env_hooks.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_destinations.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_generate_environment.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_install_python.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_libraries.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_metapackage.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_package.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_package_xml.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_python_setup.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/debug_message.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/em/order_packages.cmake.em"
-  "/opt/ros/indigo/share/catkin/cmake/em_expand.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/empy.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin-test-results.sh.develspace.in"
-  "/opt/ros/indigo/share/catkin/cmake/find_program_required.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/interrogate_setup_dot_py.py"
-  "/opt/ros/indigo/share/catkin/cmake/legacy.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/list_append_deduplicate.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/list_append_unique.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/list_insert_in_workspace_order.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/platform/lsb.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/platform/ubuntu.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/platform/windows.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/python.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/safe_execute_process.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/stamp.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/string_starts_with.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/templates/_setup_util.py.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/env.sh.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/generate_cached_setup.py.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/order_packages.context.py.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/rosinstall.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/setup.bash.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/setup.sh.in"
-  "/opt/ros/indigo/share/catkin/cmake/templates/setup.zsh.in"
-  "/opt/ros/indigo/share/catkin/cmake/test/catkin_download_test_data.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/test/gtest.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/test/nosetests.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/test/tests.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/tools/doxygen.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/tools/libraries.cmake"
-  "/opt/ros/indigo/share/catkin/cmake/tools/rt.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeCCompiler.cmake.in"
-  "/usr/share/cmake-2.8/Modules/CMakeCCompilerABI.c"
-  "/usr/share/cmake-2.8/Modules/CMakeCInformation.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeCXXCompiler.cmake.in"
-  "/usr/share/cmake-2.8/Modules/CMakeCXXCompilerABI.cpp"
-  "/usr/share/cmake-2.8/Modules/CMakeCXXInformation.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeClDeps.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeCommonLanguageInclude.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeConfigurableFile.in"
-  "/usr/share/cmake-2.8/Modules/CMakeDetermineCCompiler.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeDetermineCXXCompiler.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeDetermineCompiler.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerABI.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeDetermineCompilerId.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeDetermineSystem.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeFindBinUtils.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeGenericSystem.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeParseArguments.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeParseImplicitLinkInfo.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeSystem.cmake.in"
-  "/usr/share/cmake-2.8/Modules/CMakeSystemSpecificInformation.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeTestCCompiler.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeTestCXXCompiler.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeTestCompilerCommon.cmake"
-  "/usr/share/cmake-2.8/Modules/CMakeUnixFindMake.cmake"
-  "/usr/share/cmake-2.8/Modules/CheckFunctionExists.c"
-  "/usr/share/cmake-2.8/Modules/CheckIncludeFiles.cmake"
-  "/usr/share/cmake-2.8/Modules/CheckLibraryExists.cmake"
-  "/usr/share/cmake-2.8/Modules/CheckSymbolExists.cmake"
-  "/usr/share/cmake-2.8/Modules/Compiler/GNU-C.cmake"
-  "/usr/share/cmake-2.8/Modules/Compiler/GNU-CXX.cmake"
-  "/usr/share/cmake-2.8/Modules/Compiler/GNU.cmake"
-  "/usr/share/cmake-2.8/Modules/FindGTest.cmake"
-  "/usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake"
-  "/usr/share/cmake-2.8/Modules/FindPackageMessage.cmake"
-  "/usr/share/cmake-2.8/Modules/FindPythonInterp.cmake"
-  "/usr/share/cmake-2.8/Modules/FindThreads.cmake"
-  "/usr/share/cmake-2.8/Modules/MultiArchCross.cmake"
-  "/usr/share/cmake-2.8/Modules/Platform/Linux-CXX.cmake"
-  "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-C.cmake"
-  "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU-CXX.cmake"
-  "/usr/share/cmake-2.8/Modules/Platform/Linux-GNU.cmake"
-  "/usr/share/cmake-2.8/Modules/Platform/Linux.cmake"
-  "/usr/share/cmake-2.8/Modules/Platform/UnixPaths.cmake"
-  "/usr/src/gtest/CMakeLists.txt"
-  "/usr/src/gtest/cmake/internal_utils.cmake"
-  )
-
-# The corresponding makefile is:
-SET(CMAKE_MAKEFILE_OUTPUTS
-  "Makefile"
-  "CMakeFiles/cmake.check_cache"
-  )
-
-# Byproducts of CMake generate step:
-SET(CMAKE_MAKEFILE_PRODUCTS
-  "CMakeFiles/2.8.12.2/CMakeSystem.cmake"
-  "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake"
-  "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake"
-  "CMakeFiles/2.8.12.2/CMakeCCompiler.cmake"
-  "CMakeFiles/2.8.12.2/CMakeCXXCompiler.cmake"
-  "catkin_generated/stamps/Project/package.xml.stamp"
-  "catkin_generated/installspace/_setup_util.py"
-  "catkin_generated/installspace/env.sh"
-  "catkin_generated/installspace/setup.bash"
-  "catkin_generated/installspace/setup.sh"
-  "catkin_generated/installspace/setup.zsh"
-  "catkin_generated/installspace/.rosinstall"
-  "catkin_generated/generate_cached_setup.py"
-  "catkin_generated/env_cached.sh"
-  "catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp"
-  "catkin_generated/order_packages.py"
-  "catkin_generated/stamps/Project/order_packages.cmake.em.stamp"
-  "CMakeFiles/CMakeDirectoryInformation.cmake"
-  "gtest/CMakeFiles/CMakeDirectoryInformation.cmake"
-  )
-
-# Dependency information for all targets:
-SET(CMAKE_DEPEND_INFO_FILES
-  "CMakeFiles/clean_test_results.dir/DependInfo.cmake"
-  "CMakeFiles/doxygen.dir/DependInfo.cmake"
-  "CMakeFiles/run_tests.dir/DependInfo.cmake"
-  "CMakeFiles/tests.dir/DependInfo.cmake"
-  "gtest/CMakeFiles/gtest.dir/DependInfo.cmake"
-  "gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake"
-  )
diff --git a/build/CMakeFiles/Makefile2 b/build/CMakeFiles/Makefile2
deleted file mode 100644
index 2a11fe8ef9..0000000000
--- a/build/CMakeFiles/Makefile2
+++ /dev/null
@@ -1,266 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# Default target executed when no arguments are given to make.
-default_target: all
-.PHONY : default_target
-
-# The main recursive all target
-all:
-.PHONY : all
-
-# The main recursive preinstall target
-preinstall:
-.PHONY : preinstall
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-#=============================================================================
-# Target rules for target CMakeFiles/clean_test_results.dir
-
-# All Build rule for target.
-CMakeFiles/clean_test_results.dir/all:
-	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/depend
-	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
-	@echo "Built target clean_test_results"
-.PHONY : CMakeFiles/clean_test_results.dir/all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/clean_test_results.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/clean_test_results.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : CMakeFiles/clean_test_results.dir/rule
-
-# Convenience name for target.
-clean_test_results: CMakeFiles/clean_test_results.dir/rule
-.PHONY : clean_test_results
-
-# clean rule for target.
-CMakeFiles/clean_test_results.dir/clean:
-	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/clean
-.PHONY : CMakeFiles/clean_test_results.dir/clean
-
-# clean rule for target.
-clean: CMakeFiles/clean_test_results.dir/clean
-.PHONY : clean
-
-#=============================================================================
-# Target rules for target CMakeFiles/doxygen.dir
-
-# All Build rule for target.
-CMakeFiles/doxygen.dir/all:
-	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/depend
-	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
-	@echo "Built target doxygen"
-.PHONY : CMakeFiles/doxygen.dir/all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/doxygen.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/doxygen.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : CMakeFiles/doxygen.dir/rule
-
-# Convenience name for target.
-doxygen: CMakeFiles/doxygen.dir/rule
-.PHONY : doxygen
-
-# clean rule for target.
-CMakeFiles/doxygen.dir/clean:
-	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/clean
-.PHONY : CMakeFiles/doxygen.dir/clean
-
-# clean rule for target.
-clean: CMakeFiles/doxygen.dir/clean
-.PHONY : clean
-
-#=============================================================================
-# Target rules for target CMakeFiles/run_tests.dir
-
-# All Build rule for target.
-CMakeFiles/run_tests.dir/all:
-	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/depend
-	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
-	@echo "Built target run_tests"
-.PHONY : CMakeFiles/run_tests.dir/all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/run_tests.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/run_tests.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : CMakeFiles/run_tests.dir/rule
-
-# Convenience name for target.
-run_tests: CMakeFiles/run_tests.dir/rule
-.PHONY : run_tests
-
-# clean rule for target.
-CMakeFiles/run_tests.dir/clean:
-	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/clean
-.PHONY : CMakeFiles/run_tests.dir/clean
-
-# clean rule for target.
-clean: CMakeFiles/run_tests.dir/clean
-.PHONY : clean
-
-#=============================================================================
-# Target rules for target CMakeFiles/tests.dir
-
-# All Build rule for target.
-CMakeFiles/tests.dir/all:
-	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/depend
-	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles 
-	@echo "Built target tests"
-.PHONY : CMakeFiles/tests.dir/all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/tests.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-	$(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/tests.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : CMakeFiles/tests.dir/rule
-
-# Convenience name for target.
-tests: CMakeFiles/tests.dir/rule
-.PHONY : tests
-
-# clean rule for target.
-CMakeFiles/tests.dir/clean:
-	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/clean
-.PHONY : CMakeFiles/tests.dir/clean
-
-# clean rule for target.
-clean: CMakeFiles/tests.dir/clean
-.PHONY : clean
-
-#=============================================================================
-# Directory level rules for directory gtest
-
-# Convenience name for "all" pass in the directory.
-gtest/all:
-.PHONY : gtest/all
-
-# Convenience name for "clean" pass in the directory.
-gtest/clean: gtest/CMakeFiles/gtest.dir/clean
-gtest/clean: gtest/CMakeFiles/gtest_main.dir/clean
-.PHONY : gtest/clean
-
-# Convenience name for "preinstall" pass in the directory.
-gtest/preinstall:
-.PHONY : gtest/preinstall
-
-#=============================================================================
-# Target rules for target gtest/CMakeFiles/gtest.dir
-
-# All Build rule for target.
-gtest/CMakeFiles/gtest.dir/all:
-	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/depend
-	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles  1
-	@echo "Built target gtest"
-.PHONY : gtest/CMakeFiles/gtest.dir/all
-
-# Build rule for subdir invocation for target.
-gtest/CMakeFiles/gtest.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 1
-	$(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : gtest/CMakeFiles/gtest.dir/rule
-
-# Convenience name for target.
-gtest: gtest/CMakeFiles/gtest.dir/rule
-.PHONY : gtest
-
-# clean rule for target.
-gtest/CMakeFiles/gtest.dir/clean:
-	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/clean
-.PHONY : gtest/CMakeFiles/gtest.dir/clean
-
-# clean rule for target.
-clean: gtest/CMakeFiles/gtest.dir/clean
-.PHONY : clean
-
-#=============================================================================
-# Target rules for target gtest/CMakeFiles/gtest_main.dir
-
-# All Build rule for target.
-gtest/CMakeFiles/gtest_main.dir/all: gtest/CMakeFiles/gtest.dir/all
-	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/depend
-	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles  2
-	@echo "Built target gtest_main"
-.PHONY : gtest/CMakeFiles/gtest_main.dir/all
-
-# Build rule for subdir invocation for target.
-gtest/CMakeFiles/gtest_main.dir/rule: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 2
-	$(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : gtest/CMakeFiles/gtest_main.dir/rule
-
-# Convenience name for target.
-gtest_main: gtest/CMakeFiles/gtest_main.dir/rule
-.PHONY : gtest_main
-
-# clean rule for target.
-gtest/CMakeFiles/gtest_main.dir/clean:
-	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/clean
-.PHONY : gtest/CMakeFiles/gtest_main.dir/clean
-
-# clean rule for target.
-clean: gtest/CMakeFiles/gtest_main.dir/clean
-.PHONY : clean
-
-#=============================================================================
-# Special targets to cleanup operation of make.
-
-# Special rule to run CMake to check the build system integrity.
-# No rule that depends on this can have commands that come from listfiles
-# because they might be regenerated.
-cmake_check_build_system:
-	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
-.PHONY : cmake_check_build_system
-
diff --git a/build/CMakeFiles/TargetDirectories.txt b/build/CMakeFiles/TargetDirectories.txt
deleted file mode 100644
index 552e0c540a..0000000000
--- a/build/CMakeFiles/TargetDirectories.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-/home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir
-/home/roman/src/Firmware/build/CMakeFiles/doxygen.dir
-/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir
-/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir
-/home/roman/src/Firmware/build/CMakeFiles/run_tests.dir
-/home/roman/src/Firmware/build/CMakeFiles/tests.dir
diff --git a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake b/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake
deleted file mode 100644
index 7aff3a53c3..0000000000
--- a/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake
+++ /dev/null
@@ -1,20 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-SET(CMAKE_DEPENDS_LANGUAGES
-  )
-# The set of files for implicit dependencies of each language:
-
-# Preprocessor definitions for this target.
-SET(CMAKE_TARGET_DEFINITIONS
-  "ROS_BUILD_SHARED_LIBS=1"
-  )
-
-# Targets to which this target links.
-SET(CMAKE_TARGET_LINKED_INFO_FILES
-  )
-
-# The include file search paths:
-SET(CMAKE_C_TARGET_INCLUDE_PATH
-  )
-SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/clean_test_results.dir/build.make b/build/CMakeFiles/clean_test_results.dir/build.make
deleted file mode 100644
index b3776d0df4..0000000000
--- a/build/CMakeFiles/clean_test_results.dir/build.make
+++ /dev/null
@@ -1,66 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-# Utility rule file for clean_test_results.
-
-# Include the progress variables for this target.
-include CMakeFiles/clean_test_results.dir/progress.make
-
-CMakeFiles/clean_test_results:
-	/usr/bin/cmake -E remove_directory /home/roman/src/Firmware/build/test_results
-
-clean_test_results: CMakeFiles/clean_test_results
-clean_test_results: CMakeFiles/clean_test_results.dir/build.make
-.PHONY : clean_test_results
-
-# Rule to build all files generated by this target.
-CMakeFiles/clean_test_results.dir/build: clean_test_results
-.PHONY : CMakeFiles/clean_test_results.dir/build
-
-CMakeFiles/clean_test_results.dir/clean:
-	$(CMAKE_COMMAND) -P CMakeFiles/clean_test_results.dir/cmake_clean.cmake
-.PHONY : CMakeFiles/clean_test_results.dir/clean
-
-CMakeFiles/clean_test_results.dir/depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/clean_test_results.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : CMakeFiles/clean_test_results.dir/depend
-
diff --git a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake b/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake
deleted file mode 100644
index 46c1cb3380..0000000000
--- a/build/CMakeFiles/clean_test_results.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,8 +0,0 @@
-FILE(REMOVE_RECURSE
-  "CMakeFiles/clean_test_results"
-)
-
-# Per-language clean rules from dependency scanning.
-FOREACH(lang)
-  INCLUDE(CMakeFiles/clean_test_results.dir/cmake_clean_${lang}.cmake OPTIONAL)
-ENDFOREACH(lang)
diff --git a/build/CMakeFiles/clean_test_results.dir/progress.make b/build/CMakeFiles/clean_test_results.dir/progress.make
deleted file mode 100644
index 8b13789179..0000000000
--- a/build/CMakeFiles/clean_test_results.dir/progress.make
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/build/CMakeFiles/cmake.check_cache b/build/CMakeFiles/cmake.check_cache
deleted file mode 100644
index 3dccd73172..0000000000
--- a/build/CMakeFiles/cmake.check_cache
+++ /dev/null
@@ -1 +0,0 @@
-# This file is generated by cmake for dependency checking of the CMakeCache.txt file
diff --git a/build/CMakeFiles/doxygen.dir/DependInfo.cmake b/build/CMakeFiles/doxygen.dir/DependInfo.cmake
deleted file mode 100644
index 7aff3a53c3..0000000000
--- a/build/CMakeFiles/doxygen.dir/DependInfo.cmake
+++ /dev/null
@@ -1,20 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-SET(CMAKE_DEPENDS_LANGUAGES
-  )
-# The set of files for implicit dependencies of each language:
-
-# Preprocessor definitions for this target.
-SET(CMAKE_TARGET_DEFINITIONS
-  "ROS_BUILD_SHARED_LIBS=1"
-  )
-
-# Targets to which this target links.
-SET(CMAKE_TARGET_LINKED_INFO_FILES
-  )
-
-# The include file search paths:
-SET(CMAKE_C_TARGET_INCLUDE_PATH
-  )
-SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/doxygen.dir/build.make b/build/CMakeFiles/doxygen.dir/build.make
deleted file mode 100644
index f5cc02bb39..0000000000
--- a/build/CMakeFiles/doxygen.dir/build.make
+++ /dev/null
@@ -1,65 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-# Utility rule file for doxygen.
-
-# Include the progress variables for this target.
-include CMakeFiles/doxygen.dir/progress.make
-
-CMakeFiles/doxygen:
-
-doxygen: CMakeFiles/doxygen
-doxygen: CMakeFiles/doxygen.dir/build.make
-.PHONY : doxygen
-
-# Rule to build all files generated by this target.
-CMakeFiles/doxygen.dir/build: doxygen
-.PHONY : CMakeFiles/doxygen.dir/build
-
-CMakeFiles/doxygen.dir/clean:
-	$(CMAKE_COMMAND) -P CMakeFiles/doxygen.dir/cmake_clean.cmake
-.PHONY : CMakeFiles/doxygen.dir/clean
-
-CMakeFiles/doxygen.dir/depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/doxygen.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : CMakeFiles/doxygen.dir/depend
-
diff --git a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake b/build/CMakeFiles/doxygen.dir/cmake_clean.cmake
deleted file mode 100644
index 3cf72d90fa..0000000000
--- a/build/CMakeFiles/doxygen.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,8 +0,0 @@
-FILE(REMOVE_RECURSE
-  "CMakeFiles/doxygen"
-)
-
-# Per-language clean rules from dependency scanning.
-FOREACH(lang)
-  INCLUDE(CMakeFiles/doxygen.dir/cmake_clean_${lang}.cmake OPTIONAL)
-ENDFOREACH(lang)
diff --git a/build/CMakeFiles/doxygen.dir/progress.make b/build/CMakeFiles/doxygen.dir/progress.make
deleted file mode 100644
index 8b13789179..0000000000
--- a/build/CMakeFiles/doxygen.dir/progress.make
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/build/CMakeFiles/progress.marks b/build/CMakeFiles/progress.marks
deleted file mode 100644
index 573541ac97..0000000000
--- a/build/CMakeFiles/progress.marks
+++ /dev/null
@@ -1 +0,0 @@
-0
diff --git a/build/CMakeFiles/run_tests.dir/DependInfo.cmake b/build/CMakeFiles/run_tests.dir/DependInfo.cmake
deleted file mode 100644
index 7aff3a53c3..0000000000
--- a/build/CMakeFiles/run_tests.dir/DependInfo.cmake
+++ /dev/null
@@ -1,20 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-SET(CMAKE_DEPENDS_LANGUAGES
-  )
-# The set of files for implicit dependencies of each language:
-
-# Preprocessor definitions for this target.
-SET(CMAKE_TARGET_DEFINITIONS
-  "ROS_BUILD_SHARED_LIBS=1"
-  )
-
-# Targets to which this target links.
-SET(CMAKE_TARGET_LINKED_INFO_FILES
-  )
-
-# The include file search paths:
-SET(CMAKE_C_TARGET_INCLUDE_PATH
-  )
-SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/run_tests.dir/build.make b/build/CMakeFiles/run_tests.dir/build.make
deleted file mode 100644
index 3907c75732..0000000000
--- a/build/CMakeFiles/run_tests.dir/build.make
+++ /dev/null
@@ -1,65 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-# Utility rule file for run_tests.
-
-# Include the progress variables for this target.
-include CMakeFiles/run_tests.dir/progress.make
-
-CMakeFiles/run_tests:
-
-run_tests: CMakeFiles/run_tests
-run_tests: CMakeFiles/run_tests.dir/build.make
-.PHONY : run_tests
-
-# Rule to build all files generated by this target.
-CMakeFiles/run_tests.dir/build: run_tests
-.PHONY : CMakeFiles/run_tests.dir/build
-
-CMakeFiles/run_tests.dir/clean:
-	$(CMAKE_COMMAND) -P CMakeFiles/run_tests.dir/cmake_clean.cmake
-.PHONY : CMakeFiles/run_tests.dir/clean
-
-CMakeFiles/run_tests.dir/depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/run_tests.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : CMakeFiles/run_tests.dir/depend
-
diff --git a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake b/build/CMakeFiles/run_tests.dir/cmake_clean.cmake
deleted file mode 100644
index 45a3e057b2..0000000000
--- a/build/CMakeFiles/run_tests.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,8 +0,0 @@
-FILE(REMOVE_RECURSE
-  "CMakeFiles/run_tests"
-)
-
-# Per-language clean rules from dependency scanning.
-FOREACH(lang)
-  INCLUDE(CMakeFiles/run_tests.dir/cmake_clean_${lang}.cmake OPTIONAL)
-ENDFOREACH(lang)
diff --git a/build/CMakeFiles/run_tests.dir/progress.make b/build/CMakeFiles/run_tests.dir/progress.make
deleted file mode 100644
index 8b13789179..0000000000
--- a/build/CMakeFiles/run_tests.dir/progress.make
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/build/CMakeFiles/tests.dir/DependInfo.cmake b/build/CMakeFiles/tests.dir/DependInfo.cmake
deleted file mode 100644
index 7aff3a53c3..0000000000
--- a/build/CMakeFiles/tests.dir/DependInfo.cmake
+++ /dev/null
@@ -1,20 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-SET(CMAKE_DEPENDS_LANGUAGES
-  )
-# The set of files for implicit dependencies of each language:
-
-# Preprocessor definitions for this target.
-SET(CMAKE_TARGET_DEFINITIONS
-  "ROS_BUILD_SHARED_LIBS=1"
-  )
-
-# Targets to which this target links.
-SET(CMAKE_TARGET_LINKED_INFO_FILES
-  )
-
-# The include file search paths:
-SET(CMAKE_C_TARGET_INCLUDE_PATH
-  )
-SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/CMakeFiles/tests.dir/build.make b/build/CMakeFiles/tests.dir/build.make
deleted file mode 100644
index 720d530001..0000000000
--- a/build/CMakeFiles/tests.dir/build.make
+++ /dev/null
@@ -1,65 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-# Utility rule file for tests.
-
-# Include the progress variables for this target.
-include CMakeFiles/tests.dir/progress.make
-
-CMakeFiles/tests:
-
-tests: CMakeFiles/tests
-tests: CMakeFiles/tests.dir/build.make
-.PHONY : tests
-
-# Rule to build all files generated by this target.
-CMakeFiles/tests.dir/build: tests
-.PHONY : CMakeFiles/tests.dir/build
-
-CMakeFiles/tests.dir/clean:
-	$(CMAKE_COMMAND) -P CMakeFiles/tests.dir/cmake_clean.cmake
-.PHONY : CMakeFiles/tests.dir/clean
-
-CMakeFiles/tests.dir/depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /home/roman/src/Firmware/src /home/roman/src/Firmware/build /home/roman/src/Firmware/build /home/roman/src/Firmware/build/CMakeFiles/tests.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : CMakeFiles/tests.dir/depend
-
diff --git a/build/CMakeFiles/tests.dir/cmake_clean.cmake b/build/CMakeFiles/tests.dir/cmake_clean.cmake
deleted file mode 100644
index a0424cfc7c..0000000000
--- a/build/CMakeFiles/tests.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,8 +0,0 @@
-FILE(REMOVE_RECURSE
-  "CMakeFiles/tests"
-)
-
-# Per-language clean rules from dependency scanning.
-FOREACH(lang)
-  INCLUDE(CMakeFiles/tests.dir/cmake_clean_${lang}.cmake OPTIONAL)
-ENDFOREACH(lang)
diff --git a/build/CMakeFiles/tests.dir/progress.make b/build/CMakeFiles/tests.dir/progress.make
deleted file mode 100644
index 8b13789179..0000000000
--- a/build/CMakeFiles/tests.dir/progress.make
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/build/CTestTestfile.cmake b/build/CTestTestfile.cmake
deleted file mode 100644
index 7b55348244..0000000000
--- a/build/CTestTestfile.cmake
+++ /dev/null
@@ -1,7 +0,0 @@
-# CMake generated Testfile for 
-# Source directory: /home/roman/src/Firmware/src
-# Build directory: /home/roman/src/Firmware/build
-# 
-# This file includes the relevant testing commands required for 
-# testing this directory and lists subdirectories to be tested as well.
-SUBDIRS(gtest)
diff --git a/build/Makefile b/build/Makefile
deleted file mode 100644
index c7d796cc42..0000000000
--- a/build/Makefile
+++ /dev/null
@@ -1,262 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# Default target executed when no arguments are given to make.
-default_target: all
-.PHONY : default_target
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-#=============================================================================
-# Targets provided globally by CMake.
-
-# Special rule for the target edit_cache
-edit_cache:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..."
-	/usr/bin/cmake -i .
-.PHONY : edit_cache
-
-# Special rule for the target edit_cache
-edit_cache/fast: edit_cache
-.PHONY : edit_cache/fast
-
-# Special rule for the target install
-install: preinstall
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
-	/usr/bin/cmake -P cmake_install.cmake
-.PHONY : install
-
-# Special rule for the target install
-install/fast: preinstall/fast
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
-	/usr/bin/cmake -P cmake_install.cmake
-.PHONY : install/fast
-
-# Special rule for the target install/local
-install/local: preinstall
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..."
-	/usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake
-.PHONY : install/local
-
-# Special rule for the target install/local
-install/local/fast: install/local
-.PHONY : install/local/fast
-
-# Special rule for the target install/strip
-install/strip: preinstall
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..."
-	/usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake
-.PHONY : install/strip
-
-# Special rule for the target install/strip
-install/strip/fast: install/strip
-.PHONY : install/strip/fast
-
-# Special rule for the target list_install_components
-list_install_components:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\""
-.PHONY : list_install_components
-
-# Special rule for the target list_install_components
-list_install_components/fast: list_install_components
-.PHONY : list_install_components/fast
-
-# Special rule for the target rebuild_cache
-rebuild_cache:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
-	/usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
-.PHONY : rebuild_cache
-
-# Special rule for the target rebuild_cache
-rebuild_cache/fast: rebuild_cache
-.PHONY : rebuild_cache/fast
-
-# Special rule for the target test
-test:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..."
-	/usr/bin/ctest --force-new-ctest-process $(ARGS)
-.PHONY : test
-
-# Special rule for the target test
-test/fast: test
-.PHONY : test/fast
-
-# The main all target
-all: cmake_check_build_system
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/CMakeFiles/progress.marks
-	$(MAKE) -f CMakeFiles/Makefile2 all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : all
-
-# The main clean target
-clean:
-	$(MAKE) -f CMakeFiles/Makefile2 clean
-.PHONY : clean
-
-# The main clean target
-clean/fast: clean
-.PHONY : clean/fast
-
-# Prepare targets for installation.
-preinstall: all
-	$(MAKE) -f CMakeFiles/Makefile2 preinstall
-.PHONY : preinstall
-
-# Prepare targets for installation.
-preinstall/fast:
-	$(MAKE) -f CMakeFiles/Makefile2 preinstall
-.PHONY : preinstall/fast
-
-# clear depends
-depend:
-	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
-.PHONY : depend
-
-#=============================================================================
-# Target rules for targets named clean_test_results
-
-# Build rule for target.
-clean_test_results: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 clean_test_results
-.PHONY : clean_test_results
-
-# fast build rule for target.
-clean_test_results/fast:
-	$(MAKE) -f CMakeFiles/clean_test_results.dir/build.make CMakeFiles/clean_test_results.dir/build
-.PHONY : clean_test_results/fast
-
-#=============================================================================
-# Target rules for targets named doxygen
-
-# Build rule for target.
-doxygen: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 doxygen
-.PHONY : doxygen
-
-# fast build rule for target.
-doxygen/fast:
-	$(MAKE) -f CMakeFiles/doxygen.dir/build.make CMakeFiles/doxygen.dir/build
-.PHONY : doxygen/fast
-
-#=============================================================================
-# Target rules for targets named run_tests
-
-# Build rule for target.
-run_tests: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 run_tests
-.PHONY : run_tests
-
-# fast build rule for target.
-run_tests/fast:
-	$(MAKE) -f CMakeFiles/run_tests.dir/build.make CMakeFiles/run_tests.dir/build
-.PHONY : run_tests/fast
-
-#=============================================================================
-# Target rules for targets named tests
-
-# Build rule for target.
-tests: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 tests
-.PHONY : tests
-
-# fast build rule for target.
-tests/fast:
-	$(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build
-.PHONY : tests/fast
-
-#=============================================================================
-# Target rules for targets named gtest
-
-# Build rule for target.
-gtest: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 gtest
-.PHONY : gtest
-
-# fast build rule for target.
-gtest/fast:
-	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build
-.PHONY : gtest/fast
-
-#=============================================================================
-# Target rules for targets named gtest_main
-
-# Build rule for target.
-gtest_main: cmake_check_build_system
-	$(MAKE) -f CMakeFiles/Makefile2 gtest_main
-.PHONY : gtest_main
-
-# fast build rule for target.
-gtest_main/fast:
-	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build
-.PHONY : gtest_main/fast
-
-# Help Target
-help:
-	@echo "The following are some of the valid targets for this Makefile:"
-	@echo "... all (the default if no target is provided)"
-	@echo "... clean"
-	@echo "... depend"
-	@echo "... clean_test_results"
-	@echo "... doxygen"
-	@echo "... edit_cache"
-	@echo "... install"
-	@echo "... install/local"
-	@echo "... install/strip"
-	@echo "... list_install_components"
-	@echo "... rebuild_cache"
-	@echo "... run_tests"
-	@echo "... test"
-	@echo "... tests"
-	@echo "... gtest"
-	@echo "... gtest_main"
-.PHONY : help
-
-
-
-#=============================================================================
-# Special targets to cleanup operation of make.
-
-# Special rule to run CMake to check the build system integrity.
-# No rule that depends on this can have commands that come from listfiles
-# because they might be regenerated.
-cmake_check_build_system:
-	$(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
-.PHONY : cmake_check_build_system
-
diff --git a/build/catkin/catkin_generated/version/package.cmake b/build/catkin/catkin_generated/version/package.cmake
deleted file mode 100644
index 1cbfc826b0..0000000000
--- a/build/catkin/catkin_generated/version/package.cmake
+++ /dev/null
@@ -1,9 +0,0 @@
-set(_CATKIN_CURRENT_PACKAGE "catkin")
-set(catkin_VERSION "0.6.9")
-set(catkin_BUILD_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2")
-set(catkin_BUILD_DEPENDS "python-empy" "python-argparse" "python-catkin-pkg")
-set(catkin_DEPRECATED "")
-set(catkin_RUN_DEPENDS "python-argparse" "python-catkin-pkg" "gtest" "python-empy" "python-nose")
-set(catkin_MAINTAINER "Dirk Thomas <dthomas@osrfoundation.org>")
-set(catkin_BUILDTOOL_DEPENDS "cmake")
-set(catkin_RUN_DEPENDS_python-catkin-pkg_VERSION_GTE "0.2.2")
\ No newline at end of file
diff --git a/build/catkin_generated/env_cached.sh b/build/catkin_generated/env_cached.sh
deleted file mode 100755
index d6be91db5c..0000000000
--- a/build/catkin_generated/env_cached.sh
+++ /dev/null
@@ -1,16 +0,0 @@
-#!/usr/bin/env sh
-# generated from catkin/cmake/templates/env.sh.in
-
-if [ $# -eq 0 ] ; then
-  /bin/echo "Usage: env.sh COMMANDS"
-  /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
-  exit 1
-fi
-
-# ensure to not use different shell type which was set before
-CATKIN_SHELL=sh
-
-# source setup_cached.sh from same directory as this file
-_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
-. "$_CATKIN_SETUP_DIR/setup_cached.sh"
-exec "$@"
diff --git a/build/catkin_generated/generate_cached_setup.py b/build/catkin_generated/generate_cached_setup.py
deleted file mode 100644
index 7a56cdb7f9..0000000000
--- a/build/catkin_generated/generate_cached_setup.py
+++ /dev/null
@@ -1,29 +0,0 @@
-from __future__ import print_function
-import argparse
-import os
-import stat
-import sys
-
-# find the import for catkin's python package - either from source space or from an installed underlay
-if os.path.exists(os.path.join('/opt/ros/indigo/share/catkin/cmake', 'catkinConfig.cmake.in')):
-    sys.path.insert(0, os.path.join('/opt/ros/indigo/share/catkin/cmake', '..', 'python'))
-try:
-    from catkin.environment_cache import generate_environment_script
-except ImportError:
-    # search for catkin package in all workspaces and prepend to path
-    for workspace in "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';'):
-        python_path = os.path.join(workspace, 'lib/python2.7/dist-packages')
-        if os.path.isdir(os.path.join(python_path, 'catkin')):
-            sys.path.insert(0, python_path)
-            break
-    from catkin.environment_cache import generate_environment_script
-
-code = generate_environment_script('/home/roman/src/Firmware/devel/env.sh')
-
-output_filename = '/home/roman/src/Firmware/build/catkin_generated/setup_cached.sh'
-with open(output_filename, 'w') as f:
-    #print('Generate script for cached setup "%s"' % output_filename)
-    f.write('\n'.join(code))
-
-mode = os.stat(output_filename).st_mode
-os.chmod(output_filename, mode | stat.S_IXUSR)
diff --git a/build/catkin_generated/installspace/.rosinstall b/build/catkin_generated/installspace/.rosinstall
deleted file mode 100644
index a959c71efc..0000000000
--- a/build/catkin_generated/installspace/.rosinstall
+++ /dev/null
@@ -1,2 +0,0 @@
-- setup-file:
-    local-name: /home/roman/src/Firmware/install/setup.sh
diff --git a/build/catkin_generated/installspace/_setup_util.py b/build/catkin_generated/installspace/_setup_util.py
deleted file mode 100755
index 8db644140c..0000000000
--- a/build/catkin_generated/installspace/_setup_util.py
+++ /dev/null
@@ -1,287 +0,0 @@
-#!/usr/bin/python
-
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * 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.
-#  * Neither the name of Willow Garage, Inc. 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.
-
-'''This file generates shell code for the setup.SHELL scripts to set environment variables'''
-
-from __future__ import print_function
-import argparse
-import copy
-import errno
-import os
-import platform
-import sys
-
-CATKIN_MARKER_FILE = '.catkin'
-
-system = platform.system()
-IS_DARWIN = (system == 'Darwin')
-IS_WINDOWS = (system == 'Windows')
-
-# subfolder of workspace prepended to CMAKE_PREFIX_PATH
-ENV_VAR_SUBFOLDERS = {
-    'CMAKE_PREFIX_PATH': '',
-    'CPATH': 'include',
-    'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')],
-    'PATH': 'bin',
-    'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')],
-    'PYTHONPATH': 'lib/python2.7/dist-packages',
-}
-
-
-def rollback_env_variables(environ, env_var_subfolders):
-    '''
-    Generate shell code to reset environment variables
-    by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH.
-    This does not cover modifications performed by environment hooks.
-    '''
-    lines = []
-    unmodified_environ = copy.copy(environ)
-    for key in sorted(env_var_subfolders.keys()):
-        subfolders = env_var_subfolders[key]
-        if not isinstance(subfolders, list):
-            subfolders = [subfolders]
-        for subfolder in subfolders:
-            value = _rollback_env_variable(unmodified_environ, key, subfolder)
-            if value is not None:
-                environ[key] = value
-                lines.append(assignment(key, value))
-    if lines:
-        lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH'))
-    return lines
-
-
-def _rollback_env_variable(environ, name, subfolder):
-    '''
-    For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder.
-
-    :param subfolder: str '' or subfoldername that may start with '/'
-    :returns: the updated value of the environment variable.
-    '''
-    value = environ[name] if name in environ else ''
-    env_paths = [path for path in value.split(os.pathsep) if path]
-    value_modified = False
-    if subfolder:
-        if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)):
-            subfolder = subfolder[1:]
-        if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)):
-            subfolder = subfolder[:-1]
-    for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True):
-        path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path
-        path_to_remove = None
-        for env_path in env_paths:
-            env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path
-            if env_path_clean == path_to_find:
-                path_to_remove = env_path
-                break
-        if path_to_remove:
-            env_paths.remove(path_to_remove)
-            value_modified = True
-    new_value = os.pathsep.join(env_paths)
-    return new_value if value_modified else None
-
-
-def _get_workspaces(environ, include_fuerte=False, include_non_existing=False):
-    '''
-    Based on CMAKE_PREFIX_PATH return all catkin workspaces.
-
-    :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool``
-    '''
-    # get all cmake prefix paths
-    env_name = 'CMAKE_PREFIX_PATH'
-    value = environ[env_name] if env_name in environ else ''
-    paths = [path for path in value.split(os.pathsep) if path]
-    # remove non-workspace paths
-    workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))]
-    return workspaces
-
-
-def prepend_env_variables(environ, env_var_subfolders, workspaces):
-    '''
-    Generate shell code to prepend environment variables
-    for the all workspaces.
-    '''
-    lines = []
-    lines.append(comment('prepend folders of workspaces to environment variables'))
-
-    paths = [path for path in workspaces.split(os.pathsep) if path]
-
-    prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '')
-    lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix))
-
-    for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']):
-        subfolder = env_var_subfolders[key]
-        prefix = _prefix_env_variable(environ, key, paths, subfolder)
-        lines.append(prepend(environ, key, prefix))
-    return lines
-
-
-def _prefix_env_variable(environ, name, paths, subfolders):
-    '''
-    Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items.
-    '''
-    value = environ[name] if name in environ else ''
-    environ_paths = [path for path in value.split(os.pathsep) if path]
-    checked_paths = []
-    for path in paths:
-        if not isinstance(subfolders, list):
-            subfolders = [subfolders]
-        for subfolder in subfolders:
-            path_tmp = path
-            if subfolder:
-                path_tmp = os.path.join(path_tmp, subfolder)
-            # exclude any path already in env and any path we already added
-            if path_tmp not in environ_paths and path_tmp not in checked_paths:
-                checked_paths.append(path_tmp)
-    prefix_str = os.pathsep.join(checked_paths)
-    if prefix_str != '' and environ_paths:
-        prefix_str += os.pathsep
-    return prefix_str
-
-
-def assignment(key, value):
-    if not IS_WINDOWS:
-        return 'export %s="%s"' % (key, value)
-    else:
-        return 'set %s=%s' % (key, value)
-
-
-def comment(msg):
-    if not IS_WINDOWS:
-        return '# %s' % msg
-    else:
-        return 'REM %s' % msg
-
-
-def prepend(environ, key, prefix):
-    if key not in environ or not environ[key]:
-        return assignment(key, prefix)
-    if not IS_WINDOWS:
-        return 'export %s="%s$%s"' % (key, prefix, key)
-    else:
-        return 'set %s=%s%%%s%%' % (key, prefix, key)
-
-
-def find_env_hooks(environ, cmake_prefix_path):
-    '''
-    Generate shell code with found environment hooks
-    for the all workspaces.
-    '''
-    lines = []
-    lines.append(comment('found environment hooks in workspaces'))
-
-    generic_env_hooks = []
-    generic_env_hooks_workspace = []
-    specific_env_hooks = []
-    specific_env_hooks_workspace = []
-    generic_env_hooks_by_filename = {}
-    specific_env_hooks_by_filename = {}
-    generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh'
-    specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None
-    # remove non-workspace paths
-    workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))]
-    for workspace in reversed(workspaces):
-        env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d')
-        if os.path.isdir(env_hook_dir):
-            for filename in sorted(os.listdir(env_hook_dir)):
-                if filename.endswith('.%s' % generic_env_hook_ext):
-                    # remove previous env hook with same name if present
-                    if filename in generic_env_hooks_by_filename:
-                        i = generic_env_hooks.index(generic_env_hooks_by_filename[filename])
-                        generic_env_hooks.pop(i)
-                        generic_env_hooks_workspace.pop(i)
-                    # append env hook
-                    generic_env_hooks.append(os.path.join(env_hook_dir, filename))
-                    generic_env_hooks_workspace.append(workspace)
-                    generic_env_hooks_by_filename[filename] = generic_env_hooks[-1]
-                elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext):
-                    # remove previous env hook with same name if present
-                    if filename in specific_env_hooks_by_filename:
-                        i = specific_env_hooks.index(specific_env_hooks_by_filename[filename])
-                        specific_env_hooks.pop(i)
-                        specific_env_hooks_workspace.pop(i)
-                    # append env hook
-                    specific_env_hooks.append(os.path.join(env_hook_dir, filename))
-                    specific_env_hooks_workspace.append(workspace)
-                    specific_env_hooks_by_filename[filename] = specific_env_hooks[-1]
-    env_hooks = generic_env_hooks + specific_env_hooks
-    env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace
-    count = len(env_hooks)
-    lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count))
-    for i in range(count):
-        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i]))
-        lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i]))
-    return lines
-
-
-def _parse_arguments(args=None):
-    parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.')
-    parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context')
-    return parser.parse_known_args(args=args)[0]
-
-
-if __name__ == '__main__':
-    try:
-        try:
-            args = _parse_arguments()
-        except Exception as e:
-            print(e, file=sys.stderr)
-            sys.exit(1)
-
-        # environment at generation time
-        CMAKE_PREFIX_PATH = '/home/roman/catkin_ws/devel;/opt/ros/indigo'.split(';')
-        # prepend current workspace if not already part of CPP
-        base_path = os.path.dirname(__file__)
-        if base_path not in CMAKE_PREFIX_PATH:
-            CMAKE_PREFIX_PATH.insert(0, base_path)
-        CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH)
-
-        environ = dict(os.environ)
-        lines = []
-        if not args.extend:
-            lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS)
-        lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH)
-        lines += find_env_hooks(environ, CMAKE_PREFIX_PATH)
-        print('\n'.join(lines))
-
-        # need to explicitly flush the output
-        sys.stdout.flush()
-    except IOError as e:
-        # and catch potantial "broken pipe" if stdout is not writable
-        # which can happen when piping the output to a file but the disk is full
-        if e.errno == errno.EPIPE:
-            print(e, file=sys.stderr)
-            sys.exit(2)
-        raise
-
-    sys.exit(0)
diff --git a/build/catkin_generated/installspace/env.sh b/build/catkin_generated/installspace/env.sh
deleted file mode 100755
index 8aa9d244ae..0000000000
--- a/build/catkin_generated/installspace/env.sh
+++ /dev/null
@@ -1,16 +0,0 @@
-#!/usr/bin/env sh
-# generated from catkin/cmake/templates/env.sh.in
-
-if [ $# -eq 0 ] ; then
-  /bin/echo "Usage: env.sh COMMANDS"
-  /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
-  exit 1
-fi
-
-# ensure to not use different shell type which was set before
-CATKIN_SHELL=sh
-
-# source setup.sh from same directory as this file
-_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
-. "$_CATKIN_SETUP_DIR/setup.sh"
-exec "$@"
diff --git a/build/catkin_generated/installspace/setup.bash b/build/catkin_generated/installspace/setup.bash
deleted file mode 100644
index ff47af8f30..0000000000
--- a/build/catkin_generated/installspace/setup.bash
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/usr/bin/env bash
-# generated from catkin/cmake/templates/setup.bash.in
-
-CATKIN_SHELL=bash
-
-# source setup.sh from same directory as this file
-_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
-. "$_CATKIN_SETUP_DIR/setup.sh"
diff --git a/build/catkin_generated/installspace/setup.sh b/build/catkin_generated/installspace/setup.sh
deleted file mode 100644
index 95e1571bef..0000000000
--- a/build/catkin_generated/installspace/setup.sh
+++ /dev/null
@@ -1,87 +0,0 @@
-#!/usr/bin/env sh
-# generated from catkin/cmake/template/setup.sh.in
-
-# Sets various environment variables and sources additional environment hooks.
-# It tries it's best to undo changes from a previously sourced setup file before.
-# Supported command line options:
-# --extend: skips the undoing of changes from a previously sourced setup file
-
-# since this file is sourced either use the provided _CATKIN_SETUP_DIR
-# or fall back to the destination set at configure time
-: ${_CATKIN_SETUP_DIR:=/home/roman/src/Firmware/install}
-_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py"
-unset _CATKIN_SETUP_DIR
-
-if [ ! -f "$_SETUP_UTIL" ]; then
-  echo "Missing Python script: $_SETUP_UTIL"
-  return 22
-fi
-
-# detect if running on Darwin platform
-_UNAME=`uname -s`
-_IS_DARWIN=0
-if [ "$_UNAME" = "Darwin" ]; then
-  _IS_DARWIN=1
-fi
-unset _UNAME
-
-# make sure to export all environment variables
-export CMAKE_PREFIX_PATH
-export CPATH
-if [ $_IS_DARWIN -eq 0 ]; then
-  export LD_LIBRARY_PATH
-else
-  export DYLD_LIBRARY_PATH
-fi
-unset _IS_DARWIN
-export PATH
-export PKG_CONFIG_PATH
-export PYTHONPATH
-
-# remember type of shell if not already set
-if [ -z "$CATKIN_SHELL" ]; then
-  CATKIN_SHELL=sh
-fi
-
-# invoke Python script to generate necessary exports of environment variables
-_SETUP_TMP=`mktemp /tmp/setup.sh.XXXXXXXXXX`
-if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then
-  echo "Could not create temporary file: $_SETUP_TMP"
-  return 1
-fi
-CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ > $_SETUP_TMP
-_RC=$?
-if [ $_RC -ne 0 ]; then
-  if [ $_RC -eq 2 ]; then
-    echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?"
-  else
-    echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC"
-  fi
-  unset _RC
-  unset _SETUP_UTIL
-  rm -f $_SETUP_TMP
-  unset _SETUP_TMP
-  return 1
-fi
-unset _RC
-unset _SETUP_UTIL
-. $_SETUP_TMP
-rm -f $_SETUP_TMP
-unset _SETUP_TMP
-
-# source all environment hooks
-_i=0
-while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do
-  eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i
-  unset _CATKIN_ENVIRONMENT_HOOKS_$_i
-  eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
-  unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
-  # set workspace for environment hook
-  CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace
-  . "$_envfile"
-  unset CATKIN_ENV_HOOK_WORKSPACE
-  _i=$((_i + 1))
-done
-unset _i
-
-unset _CATKIN_ENVIRONMENT_HOOKS_COUNT
diff --git a/build/catkin_generated/installspace/setup.zsh b/build/catkin_generated/installspace/setup.zsh
deleted file mode 100644
index b660717663..0000000000
--- a/build/catkin_generated/installspace/setup.zsh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/usr/bin/env zsh
-# generated from catkin/cmake/templates/setup.zsh.in
-
-CATKIN_SHELL=zsh
-_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
-emulate sh # emulate POSIX
-. "$_CATKIN_SETUP_DIR/setup.sh"
-emulate zsh # back to zsh mode
diff --git a/build/catkin_generated/order_packages.cmake b/build/catkin_generated/order_packages.cmake
deleted file mode 100644
index 6b0e2dff92..0000000000
--- a/build/catkin_generated/order_packages.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-# generated from catkin/cmake/em/order_packages.cmake.em
-
-set(CATKIN_ORDERED_PACKAGES "")
-set(CATKIN_ORDERED_PACKAGE_PATHS "")
-set(CATKIN_ORDERED_PACKAGES_IS_META "")
-set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "")
-
-set(CATKIN_MESSAGE_GENERATORS )
-
-set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "/usr/lib/pymodules/python2.7/catkin_pkg/templates/metapackage.cmake.in")
diff --git a/build/catkin_generated/order_packages.py b/build/catkin_generated/order_packages.py
deleted file mode 100644
index 5cb1765802..0000000000
--- a/build/catkin_generated/order_packages.py
+++ /dev/null
@@ -1,5 +0,0 @@
-# generated from catkin/cmake/template/order_packages.context.py.in
-source_root_dir = "/home/roman/src/Firmware/src"
-whitelisted_packages = "".split(';') if "" != "" else []
-blacklisted_packages = "".split(';') if "" != "" else []
-underlay_workspaces = "/home/roman/catkin_ws/devel;/opt/ros/indigo".split(';') if "/home/roman/catkin_ws/devel;/opt/ros/indigo" != "" else []
diff --git a/build/catkin_generated/setup_cached.sh b/build/catkin_generated/setup_cached.sh
deleted file mode 100755
index e03bce4ba5..0000000000
--- a/build/catkin_generated/setup_cached.sh
+++ /dev/null
@@ -1,20 +0,0 @@
-#!/usr/bin/env sh
-# generated from catkin/python/catkin/environment_cache.py
-
-# based on a snapshot of the environment before and after calling the setup script
-# it emulates the modifications of the setup script without recurring computations
-
-# new environment variables
-
-# modified environment variables
-export CATKIN_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results"
-export CMAKE_PREFIX_PATH="/home/roman/src/Firmware/devel:$CMAKE_PREFIX_PATH"
-export CPATH="/home/roman/src/Firmware/devel/include:$CPATH"
-export LD_LIBRARY_PATH="/home/roman/src/Firmware/devel/lib:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib/x86_64-linux-gnu:/opt/ros/indigo/lib/x86_64-linux-gnu:/home/roman/catkin_ws/devel/lib:/opt/ros/indigo/lib"
-export PATH="/home/roman/src/Firmware/devel/bin:$PATH"
-export PKG_CONFIG_PATH="/home/roman/src/Firmware/devel/lib/pkgconfig:/home/roman/src/Firmware/devel/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH"
-export PWD="/home/roman/src/Firmware/build"
-export PYTHONPATH="/home/roman/src/Firmware/devel/lib/python2.7/dist-packages:$PYTHONPATH"
-export ROSLISP_PACKAGE_DIRECTORIES="/home/roman/src/Firmware/devel/share/common-lisp:$ROSLISP_PACKAGE_DIRECTORIES"
-export ROS_PACKAGE_PATH="/home/roman/src/Firmware/src:$ROS_PACKAGE_PATH"
-export ROS_TEST_RESULTS_DIR="/home/roman/src/Firmware/build/test_results"
\ No newline at end of file
diff --git a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp b/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp
deleted file mode 100644
index 26882f02aa..0000000000
--- a/build/catkin_generated/stamps/Project/interrogate_setup_dot_py.py.stamp
+++ /dev/null
@@ -1,250 +0,0 @@
-#!/usr/bin/env python
-
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * 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.
-#  * Neither the name of Willow Garage, Inc. 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.
-
-from __future__ import print_function
-import os
-import sys
-
-import distutils.core
-try:
-    import setuptools
-except ImportError:
-    pass
-
-from argparse import ArgumentParser
-
-
-def _get_locations(pkgs, package_dir):
-    """
-    based on setuptools logic and the package_dir dict, builds a dict
-    of location roots for each pkg in pkgs.
-    See http://docs.python.org/distutils/setupscript.html
-
-    :returns: a dict {pkgname: root} for each pkgname in pkgs (and each of their parents)
-    """
-    # package_dir contains a dict {package_name: relativepath}
-    # Example {'': 'src', 'foo': 'lib', 'bar': 'lib2'}
-    #
-    # '' means where to look for any package unless a parent package
-    # is listed so package bar.pot is expected at lib2/bar/pot,
-    # whereas package sup.dee is expected at src/sup/dee
-    #
-    # if package_dir does not state anything about a package,
-    # setuptool expects the package folder to be in the root of the
-    # project
-    locations = {}
-    allprefix = package_dir.get('', '')
-    for pkg in pkgs:
-        parent_location = None
-        splits = pkg.split('.')
-        # we iterate over compound name from parent to child
-        # so once we found parent, children just append to their parent
-        for key_len in range(len(splits)):
-            key = '.'.join(splits[:key_len + 1])
-            if key not in locations:
-                if key in package_dir:
-                    locations[key] = package_dir[key]
-                elif parent_location is not None:
-                    locations[key] = parent_location
-                else:
-                    locations[key] = allprefix
-            parent_location = locations[key]
-    return locations
-
-
-def generate_cmake_file(package_name, version, scripts, package_dir, pkgs, modules):
-    """
-    Generates lines to add to a cmake file which will set variables
-
-    :param version: str, format 'int.int.int'
-    :param scripts: [list of str]: relative paths to scripts
-    :param package_dir: {modulename: path}
-    :pkgs: [list of str] python_packages declared in catkin package
-    :modules: [list of str] python modules
-    """
-    prefix = '%s_SETUP_PY' % package_name
-    result = []
-    result.append(r'set(%s_VERSION "%s")' % (prefix, version))
-    result.append(r'set(%s_SCRIPTS "%s")' % (prefix, ';'.join(scripts)))
-
-    # Remove packages with '.' separators.
-    #
-    # setuptools allows specifying submodules in other folders than
-    # their parent
-    #
-    # The symlink approach of catkin does not work with such submodules.
-    # In the common case, this does not matter as the submodule is
-    # within the containing module.  We verify this assumption, and if
-    # it passes, we remove submodule packages.
-    locations = _get_locations(pkgs, package_dir)
-    for pkgname, location in locations.items():
-        if not '.' in pkgname:
-            continue
-        splits = pkgname.split('.')
-        # hack: ignore write-combining setup.py files for msg and srv files
-        if splits[1] in ['msg', 'srv']:
-            continue
-        # check every child has the same root folder as its parent
-        parent_name = '.'.join(splits[:1])
-        if location != locations[parent_name]:
-            raise RuntimeError(
-                "catkin_export_python does not support setup.py files that combine across multiple directories: %s in %s, %s in %s" % (pkgname, location, parent_name, locations[parent_name]))
-
-    # If checks pass, remove all submodules
-    pkgs = [p for p in pkgs if '.' not in p]
-
-    resolved_pkgs = []
-    for pkg in pkgs:
-        resolved_pkgs += [os.path.join(locations[pkg], pkg)]
-
-    result.append(r'set(%s_PACKAGES "%s")' % (prefix, ';'.join(pkgs)))
-    result.append(r'set(%s_PACKAGE_DIRS "%s")' % (prefix, ';'.join(resolved_pkgs).replace("\\", "/")))
-
-    # skip modules which collide with package names
-    filtered_modules = []
-    for modname in modules:
-        splits = modname.split('.')
-        # check all parents too
-        equals_package = [('.'.join(splits[:-i]) in locations) for i in range(len(splits))]
-        if any(equals_package):
-            continue
-        filtered_modules.append(modname)
-    module_locations = _get_locations(filtered_modules, package_dir)
-
-    result.append(r'set(%s_MODULES "%s")' % (prefix, ';'.join(['%s.py' % m.replace('.', '/') for m in filtered_modules])))
-    result.append(r'set(%s_MODULE_DIRS "%s")' % (prefix, ';'.join([module_locations[m] for m in filtered_modules]).replace("\\", "/")))
-
-    return result
-
-
-def _create_mock_setup_function(package_name, outfile):
-    """
-    Creates a function to call instead of distutils.core.setup or
-    setuptools.setup, which just captures some args and writes them
-    into a file that can be used from cmake
-
-    :param package_name: name of the package
-    :param outfile: filename that cmake will use afterwards
-    :returns: a function to replace disutils.core.setup and setuptools.setup
-    """
-
-    def setup(*args, **kwargs):
-        '''
-        Checks kwargs and writes a scriptfile
-        '''
-        if 'version' not in kwargs:
-            sys.stderr.write("\n*** Unable to find 'version' in setup.py of %s\n" % package_name)
-            raise RuntimeError("version not found in setup.py")
-        version = kwargs['version']
-        package_dir = kwargs.get('package_dir', {})
-
-        pkgs = kwargs.get('packages', [])
-        scripts = kwargs.get('scripts', [])
-        modules = kwargs.get('py_modules', [])
-
-        unsupported_args = [
-            'entry_points',
-            'exclude_package_data',
-            'ext_modules ',
-            'ext_package',
-            'include_package_data',
-            'namespace_packages',
-            'setup_requires',
-            'use_2to3',
-            'zip_safe']
-        used_unsupported_args = [arg for arg in unsupported_args if arg in kwargs]
-        if used_unsupported_args:
-            sys.stderr.write("*** Arguments %s to setup() not supported in catkin devel space in setup.py of %s\n" % (used_unsupported_args, package_name))
-
-        result = generate_cmake_file(package_name=package_name,
-                                     version=version,
-                                     scripts=scripts,
-                                     package_dir=package_dir,
-                                     pkgs=pkgs,
-                                     modules=modules)
-        with open(outfile, 'w') as out:
-            out.write('\n'.join(result))
-
-    return setup
-
-
-def main():
-    """
-    Script main, parses arguments and invokes Dummy.setup indirectly.
-    """
-    parser = ArgumentParser(description='Utility to read setup.py values from cmake macros. Creates a file with CMake set commands setting variables.')
-    parser.add_argument('package_name', help='Name of catkin package')
-    parser.add_argument('setupfile_path', help='Full path to setup.py')
-    parser.add_argument('outfile', help='Where to write result to')
-
-    args = parser.parse_args()
-
-    # print("%s" % sys.argv)
-    # PACKAGE_NAME = sys.argv[1]
-    # OUTFILE = sys.argv[3]
-    # print("Interrogating setup.py for package %s into %s " % (PACKAGE_NAME, OUTFILE),
-    #      file=sys.stderr)
-
-    # print("executing %s" % args.setupfile_path)
-
-    # be sure you're in the directory containing
-    # setup.py so the sys.path manipulation works,
-    # so the import of __version__ works
-    os.chdir(os.path.dirname(os.path.abspath(args.setupfile_path)))
-
-    # patch setup() function of distutils and setuptools for the
-    # context of evaluating setup.py
-    try:
-        fake_setup = _create_mock_setup_function(package_name=args.package_name,
-                                                outfile=args.outfile)
-
-        distutils_backup = distutils.core.setup
-        distutils.core.setup = fake_setup
-        try:
-            setuptools_backup = setuptools.setup
-            setuptools.setup = fake_setup
-        except NameError:
-            pass
-
-        with open(args.setupfile_path, 'r') as fh:
-            exec(fh.read())
-    finally:
-        distutils.core.setup = distutils_backup
-        try:
-            setuptools.setup = setuptools_backup
-        except NameError:
-            pass
-
-if __name__ == '__main__':
-    main()
diff --git a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp b/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp
deleted file mode 100644
index 087d4d802e..0000000000
--- a/build/catkin_generated/stamps/Project/order_packages.cmake.em.stamp
+++ /dev/null
@@ -1,56 +0,0 @@
-# generated from catkin/cmake/em/order_packages.cmake.em
-@{
-import os
-try:
-    from catkin_pkg.cmake import get_metapackage_cmake_template_path
-except ImportError as e:
-    raise RuntimeError('ImportError: "from catkin_pkg.cmake import get_metapackage_cmake_template_path" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e)
-try:
-    from catkin_pkg.topological_order import topological_order
-except ImportError as e:
-    raise RuntimeError('ImportError: "from catkin_pkg.topological_order import topological_order" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e)
-try:
-    from catkin_pkg.package import InvalidPackage
-except ImportError as e:
-    raise RuntimeError('ImportError: "from catkin_pkg.package import InvalidPackage" failed: %s\nMake sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.' % e)
-# vars defined in order_packages.context.py.in
-try:
-    ordered_packages = topological_order(os.path.normpath(source_root_dir), whitelisted=whitelisted_packages, blacklisted=blacklisted_packages, underlay_workspaces=underlay_workspaces)
-except InvalidPackage as e:
-    print('message(FATAL_ERROR "%s")' % ('%s' % e).replace('"', '\\"'))
-    ordered_packages = []
-fatal_error = False
-}@
-
-set(CATKIN_ORDERED_PACKAGES "")
-set(CATKIN_ORDERED_PACKAGE_PATHS "")
-set(CATKIN_ORDERED_PACKAGES_IS_META "")
-set(CATKIN_ORDERED_PACKAGES_BUILD_TYPE "")
-@[for path, package in ordered_packages]@
-@[if path is None]@
-message(FATAL_ERROR "Circular dependency in subset of packages:\n@package")
-@{
-fatal_error = True
-}@
-@[elif package.name != 'catkin']@
-list(APPEND CATKIN_ORDERED_PACKAGES "@(package.name)")
-list(APPEND CATKIN_ORDERED_PACKAGE_PATHS "@(path.replace('\\','/'))")
-list(APPEND CATKIN_ORDERED_PACKAGES_IS_META "@(str('metapackage' in [e.tagname for e in package.exports]))")
-list(APPEND CATKIN_ORDERED_PACKAGES_BUILD_TYPE "@(str([e.content for e in package.exports if e.tagname == 'build_type'][0]) if 'build_type' in [e.tagname for e in package.exports] else 'catkin')")
-@{
-deprecated = [e for e in package.exports if e.tagname == 'deprecated']
-}@
-@[if deprecated]@
-message("WARNING: Package '@(package.name)' is deprecated@(' (%s)' % deprecated[0].content if deprecated[0].content else '')")
-@[end if]@
-@[end if]@
-@[end for]@
-
-@[if not fatal_error]@
-@{
-message_generators = [package.name for (_, package) in ordered_packages if 'message_generator' in [e.tagname for e in package.exports]]
-}@
-set(CATKIN_MESSAGE_GENERATORS @(' '.join(message_generators)))
-@[end if]@
-
-set(CATKIN_METAPACKAGE_CMAKE_TEMPLATE "@(get_metapackage_cmake_template_path().replace('\\','/'))")
diff --git a/build/catkin_generated/stamps/Project/package.xml.stamp b/build/catkin_generated/stamps/Project/package.xml.stamp
deleted file mode 100644
index 7c2708ac3e..0000000000
--- a/build/catkin_generated/stamps/Project/package.xml.stamp
+++ /dev/null
@@ -1,37 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>catkin</name>
-  <version>0.6.9</version>
-  <description>Low-level build system macros and infrastructure for ROS.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
-  <license>BSD</license>
-
-  <url type="website">http://www.ros.org/wiki/catkin</url>
-  <url type="bugtracker">https://github.com/ros/catkin/issues</url>
-  <url type="repository">https://github.com/ros/catkin</url>
-
-  <author>Troy Straszheim</author>
-  <author>Morten Kjaergaard</author>
-  <author>Brian Gerkey</author>
-  <author>Dirk Thomas</author>
-
-  <buildtool_depend>cmake</buildtool_depend>
-  <buildtool_export_depend>cmake</buildtool_export_depend>
-
-  <depend>python-argparse</depend>
-  <depend version_gte="0.2.2">python-catkin-pkg</depend>
-
-  <build_depend>python-empy</build_depend>
-
-  <build_export_depend>gtest</build_export_depend>
-  <build_export_depend>python-empy</build_export_depend>
-  <build_export_depend>python-nose</build_export_depend>
-
-  <test_depend>python-mock</test_depend>
-  <test_depend>python-nose</test_depend>
-
-  <export>
-    <rosdoc config="rosdoc.yaml"/>
-    <architecture_independent/>
-  </export>
-</package>
diff --git a/build/catkin_make.cache b/build/catkin_make.cache
deleted file mode 100644
index 8b13789179..0000000000
--- a/build/catkin_make.cache
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/build/cmake_install.cmake b/build/cmake_install.cmake
deleted file mode 100644
index 982fd705c6..0000000000
--- a/build/cmake_install.cmake
+++ /dev/null
@@ -1,140 +0,0 @@
-# Install script for directory: /home/roman/src/Firmware/src
-
-# Set the install prefix
-IF(NOT DEFINED CMAKE_INSTALL_PREFIX)
-  SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install")
-ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX)
-STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
-# Set the install configuration name.
-IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
-  IF(BUILD_TYPE)
-    STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
-           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
-  ELSE(BUILD_TYPE)
-    SET(CMAKE_INSTALL_CONFIG_NAME "")
-  ENDIF(BUILD_TYPE)
-  MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
-ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
-
-# Set the component getting installed.
-IF(NOT CMAKE_INSTALL_COMPONENT)
-  IF(COMPONENT)
-    MESSAGE(STATUS "Install component: \"${COMPONENT}\"")
-    SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
-  ELSE(COMPONENT)
-    SET(CMAKE_INSTALL_COMPONENT)
-  ENDIF(COMPONENT)
-ENDIF(NOT CMAKE_INSTALL_COMPONENT)
-
-# Install shared libraries without execute permission?
-IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
-  SET(CMAKE_INSTALL_SO_NO_EXE "1")
-ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  
-      if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
-        file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
-      endif()
-      if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin")
-        file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "")
-      endif()
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
-   "/home/roman/src/Firmware/install/_setup_util.py")
-  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/_setup_util.py")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
-   "/home/roman/src/Firmware/install/env.sh")
-  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE PROGRAM FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/env.sh")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
-   "/home/roman/src/Firmware/install/setup.bash")
-  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.bash")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
-   "/home/roman/src/Firmware/install/setup.sh")
-  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.sh")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
-   "/home/roman/src/Firmware/install/setup.zsh")
-  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/setup.zsh")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
-   "/home/roman/src/Firmware/install/.rosinstall")
-  IF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
-  IF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-    message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
-  ENDIF (CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
-FILE(INSTALL DESTINATION "/home/roman/src/Firmware/install" TYPE FILE FILES "/home/roman/src/Firmware/build/catkin_generated/installspace/.rosinstall")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make.bash")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-  FILE(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/etc/catkin/profile.d" TYPE FILE FILES "/opt/ros/indigo/share/catkin/cmake/env-hooks/05.catkin_make_isolated.bash")
-ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
-
-IF(NOT CMAKE_INSTALL_LOCAL_ONLY)
-  # Include the install script for each subdirectory.
-  INCLUDE("/home/roman/src/Firmware/build/gtest/cmake_install.cmake")
-
-ENDIF(NOT CMAKE_INSTALL_LOCAL_ONLY)
-
-IF(CMAKE_INSTALL_COMPONENT)
-  SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
-ELSE(CMAKE_INSTALL_COMPONENT)
-  SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
-ENDIF(CMAKE_INSTALL_COMPONENT)
-
-FILE(WRITE "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "")
-FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES})
-  FILE(APPEND "/home/roman/src/Firmware/build/${CMAKE_INSTALL_MANIFEST}" "${file}\n")
-ENDFOREACH(file)
diff --git a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake b/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake
deleted file mode 100644
index 06b7c63c13..0000000000
--- a/build/gtest/CMakeFiles/CMakeDirectoryInformation.cmake
+++ /dev/null
@@ -1,16 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# Relative path conversion top directories.
-SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/usr/src/gtest")
-SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/roman/src/Firmware/build")
-
-# Force unix paths in dependencies.
-SET(CMAKE_FORCE_UNIX_PATHS 1)
-
-
-# The C and CXX include file regular expressions for this directory.
-SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
-SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
-SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
-SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake
deleted file mode 100644
index 40234804c6..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake
+++ /dev/null
@@ -1,27 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-SET(CMAKE_DEPENDS_LANGUAGES
-  "CXX"
-  )
-# The set of files for implicit dependencies of each language:
-SET(CMAKE_DEPENDS_CHECK_CXX
-  "/usr/src/gtest/src/gtest-all.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o"
-  )
-SET(CMAKE_CXX_COMPILER_ID "GNU")
-
-# Preprocessor definitions for this target.
-SET(CMAKE_TARGET_DEFINITIONS
-  "GTEST_CREATE_SHARED_LIBRARY=1"
-  )
-
-# Targets to which this target links.
-SET(CMAKE_TARGET_LINKED_INFO_FILES
-  )
-
-# The include file search paths:
-SET(CMAKE_C_TARGET_INCLUDE_PATH
-  "/usr/src/gtest/include"
-  "/usr/src/gtest"
-  )
-SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/gtest/CMakeFiles/gtest.dir/build.make b/build/gtest/CMakeFiles/gtest.dir/build.make
deleted file mode 100644
index 744feec6d6..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/build.make
+++ /dev/null
@@ -1,102 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-# Include any dependencies generated for this target.
-include gtest/CMakeFiles/gtest.dir/depend.make
-
-# Include the progress variables for this target.
-include gtest/CMakeFiles/gtest.dir/progress.make
-
-# Include the compile flags for this target's objects.
-include gtest/CMakeFiles/gtest.dir/flags.make
-
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: gtest/CMakeFiles/gtest.dir/flags.make
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o: /usr/src/gtest/src/gtest-all.cc
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1)
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o"
-	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++   $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest.dir/src/gtest-all.cc.o -c /usr/src/gtest/src/gtest-all.cc
-
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i: cmake_force
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest.dir/src/gtest-all.cc.i"
-	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest-all.cc > CMakeFiles/gtest.dir/src/gtest-all.cc.i
-
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s: cmake_force
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest.dir/src/gtest-all.cc.s"
-	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest-all.cc -o CMakeFiles/gtest.dir/src/gtest-all.cc.s
-
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires:
-.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires
-
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires
-	$(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build
-.PHONY : gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides
-
-gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.provides.build: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o
-
-# Object files for target gtest
-gtest_OBJECTS = \
-"CMakeFiles/gtest.dir/src/gtest-all.cc.o"
-
-# External object files for target gtest
-gtest_EXTERNAL_OBJECTS =
-
-gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o
-gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/build.make
-gtest/libgtest.so: gtest/CMakeFiles/gtest.dir/link.txt
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest.so"
-	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest.dir/link.txt --verbose=$(VERBOSE)
-
-# Rule to build all files generated by this target.
-gtest/CMakeFiles/gtest.dir/build: gtest/libgtest.so
-.PHONY : gtest/CMakeFiles/gtest.dir/build
-
-gtest/CMakeFiles/gtest.dir/requires: gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o.requires
-.PHONY : gtest/CMakeFiles/gtest.dir/requires
-
-gtest/CMakeFiles/gtest.dir/clean:
-	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest.dir/cmake_clean.cmake
-.PHONY : gtest/CMakeFiles/gtest.dir/clean
-
-gtest/CMakeFiles/gtest.dir/depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : gtest/CMakeFiles/gtest.dir/depend
-
diff --git a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake
deleted file mode 100644
index 015a1ee758..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-FILE(REMOVE_RECURSE
-  "CMakeFiles/gtest.dir/src/gtest-all.cc.o"
-  "libgtest.pdb"
-  "libgtest.so"
-)
-
-# Per-language clean rules from dependency scanning.
-FOREACH(lang CXX)
-  INCLUDE(CMakeFiles/gtest.dir/cmake_clean_${lang}.cmake OPTIONAL)
-ENDFOREACH(lang)
diff --git a/build/gtest/CMakeFiles/gtest.dir/depend.make b/build/gtest/CMakeFiles/gtest.dir/depend.make
deleted file mode 100644
index 37ac348dbd..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/depend.make
+++ /dev/null
@@ -1,2 +0,0 @@
-# Empty dependencies file for gtest.
-# This may be replaced when dependencies are built.
diff --git a/build/gtest/CMakeFiles/gtest.dir/flags.make b/build/gtest/CMakeFiles/gtest.dir/flags.make
deleted file mode 100644
index fa55300acd..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/flags.make
+++ /dev/null
@@ -1,8 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# compile CXX with /usr/bin/c++
-CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest     -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra
-
-CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_EXPORTS
-
diff --git a/build/gtest/CMakeFiles/gtest.dir/link.txt b/build/gtest/CMakeFiles/gtest.dir/link.txt
deleted file mode 100644
index 8745cedd2c..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/link.txt
+++ /dev/null
@@ -1 +0,0 @@
-/usr/bin/c++  -fPIC    -shared -Wl,-soname,libgtest.so -o libgtest.so CMakeFiles/gtest.dir/src/gtest-all.cc.o  -L/home/roman/src/Firmware/build/gtest/src -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src 
diff --git a/build/gtest/CMakeFiles/gtest.dir/progress.make b/build/gtest/CMakeFiles/gtest.dir/progress.make
deleted file mode 100644
index 781c7de277..0000000000
--- a/build/gtest/CMakeFiles/gtest.dir/progress.make
+++ /dev/null
@@ -1,2 +0,0 @@
-CMAKE_PROGRESS_1 = 1
-
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake b/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake
deleted file mode 100644
index 9f3beee6e1..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake
+++ /dev/null
@@ -1,28 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-SET(CMAKE_DEPENDS_LANGUAGES
-  "CXX"
-  )
-# The set of files for implicit dependencies of each language:
-SET(CMAKE_DEPENDS_CHECK_CXX
-  "/usr/src/gtest/src/gtest_main.cc" "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
-  )
-SET(CMAKE_CXX_COMPILER_ID "GNU")
-
-# Preprocessor definitions for this target.
-SET(CMAKE_TARGET_DEFINITIONS
-  "GTEST_CREATE_SHARED_LIBRARY=1"
-  )
-
-# Targets to which this target links.
-SET(CMAKE_TARGET_LINKED_INFO_FILES
-  "/home/roman/src/Firmware/build/gtest/CMakeFiles/gtest.dir/DependInfo.cmake"
-  )
-
-# The include file search paths:
-SET(CMAKE_C_TARGET_INCLUDE_PATH
-  "/usr/src/gtest/include"
-  "/usr/src/gtest"
-  )
-SET(CMAKE_CXX_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_Fortran_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
-SET(CMAKE_ASM_TARGET_INCLUDE_PATH ${CMAKE_C_TARGET_INCLUDE_PATH})
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/build.make b/build/gtest/CMakeFiles/gtest_main.dir/build.make
deleted file mode 100644
index 67a7ec0180..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/build.make
+++ /dev/null
@@ -1,103 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-# Include any dependencies generated for this target.
-include gtest/CMakeFiles/gtest_main.dir/depend.make
-
-# Include the progress variables for this target.
-include gtest/CMakeFiles/gtest_main.dir/progress.make
-
-# Include the compile flags for this target's objects.
-include gtest/CMakeFiles/gtest_main.dir/flags.make
-
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: gtest/CMakeFiles/gtest_main.dir/flags.make
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o: /usr/src/gtest/src/gtest_main.cc
-	$(CMAKE_COMMAND) -E cmake_progress_report /home/roman/src/Firmware/build/CMakeFiles $(CMAKE_PROGRESS_1)
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Building CXX object gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
-	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++   $(CXX_DEFINES) $(CXX_FLAGS) -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.o -c /usr/src/gtest/src/gtest_main.cc
-
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i: cmake_force
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/gtest_main.dir/src/gtest_main.cc.i"
-	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -E /usr/src/gtest/src/gtest_main.cc > CMakeFiles/gtest_main.dir/src/gtest_main.cc.i
-
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s: cmake_force
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/gtest_main.dir/src/gtest_main.cc.s"
-	cd /home/roman/src/Firmware/build/gtest && /usr/bin/c++  $(CXX_DEFINES) $(CXX_FLAGS) -S /usr/src/gtest/src/gtest_main.cc -o CMakeFiles/gtest_main.dir/src/gtest_main.cc.s
-
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires:
-.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires
-
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires
-	$(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build
-.PHONY : gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides
-
-gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.provides.build: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o
-
-# Object files for target gtest_main
-gtest_main_OBJECTS = \
-"CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
-
-# External object files for target gtest_main
-gtest_main_EXTERNAL_OBJECTS =
-
-gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o
-gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/build.make
-gtest/libgtest_main.so: gtest/libgtest.so
-gtest/libgtest_main.so: gtest/CMakeFiles/gtest_main.dir/link.txt
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --red --bold "Linking CXX shared library libgtest_main.so"
-	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/gtest_main.dir/link.txt --verbose=$(VERBOSE)
-
-# Rule to build all files generated by this target.
-gtest/CMakeFiles/gtest_main.dir/build: gtest/libgtest_main.so
-.PHONY : gtest/CMakeFiles/gtest_main.dir/build
-
-gtest/CMakeFiles/gtest_main.dir/requires: gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o.requires
-.PHONY : gtest/CMakeFiles/gtest_main.dir/requires
-
-gtest/CMakeFiles/gtest_main.dir/clean:
-	cd /home/roman/src/Firmware/build/gtest && $(CMAKE_COMMAND) -P CMakeFiles/gtest_main.dir/cmake_clean.cmake
-.PHONY : gtest/CMakeFiles/gtest_main.dir/clean
-
-gtest/CMakeFiles/gtest_main.dir/depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/roman/src/Firmware/src /usr/src/gtest /home/roman/src/Firmware/build /home/roman/src/Firmware/build/gtest /home/roman/src/Firmware/build/gtest/CMakeFiles/gtest_main.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : gtest/CMakeFiles/gtest_main.dir/depend
-
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake b/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake
deleted file mode 100644
index c8fe838192..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-FILE(REMOVE_RECURSE
-  "CMakeFiles/gtest_main.dir/src/gtest_main.cc.o"
-  "libgtest_main.pdb"
-  "libgtest_main.so"
-)
-
-# Per-language clean rules from dependency scanning.
-FOREACH(lang CXX)
-  INCLUDE(CMakeFiles/gtest_main.dir/cmake_clean_${lang}.cmake OPTIONAL)
-ENDFOREACH(lang)
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/depend.make b/build/gtest/CMakeFiles/gtest_main.dir/depend.make
deleted file mode 100644
index 1d67c1ab52..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/depend.make
+++ /dev/null
@@ -1,2 +0,0 @@
-# Empty dependencies file for gtest_main.
-# This may be replaced when dependencies are built.
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/flags.make b/build/gtest/CMakeFiles/gtest_main.dir/flags.make
deleted file mode 100644
index 93ab70f12f..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/flags.make
+++ /dev/null
@@ -1,8 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# compile CXX with /usr/bin/c++
-CXX_FLAGS = -fPIC -I/usr/src/gtest/include -I/usr/src/gtest     -Wall -Wshadow -DGTEST_HAS_PTHREAD=1 -fexceptions -Wextra
-
-CXX_DEFINES = -DGTEST_CREATE_SHARED_LIBRARY=1 -Dgtest_main_EXPORTS
-
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/link.txt b/build/gtest/CMakeFiles/gtest_main.dir/link.txt
deleted file mode 100644
index d35b7f61ac..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/link.txt
+++ /dev/null
@@ -1 +0,0 @@
-/usr/bin/c++  -fPIC    -shared -Wl,-soname,libgtest_main.so -o libgtest_main.so CMakeFiles/gtest_main.dir/src/gtest_main.cc.o  -L/home/roman/src/Firmware/build/gtest/src -lpthread libgtest.so -lpthread -Wl,-rpath,/home/roman/src/Firmware/build/gtest/src:/home/roman/src/Firmware/build/gtest 
diff --git a/build/gtest/CMakeFiles/gtest_main.dir/progress.make b/build/gtest/CMakeFiles/gtest_main.dir/progress.make
deleted file mode 100644
index 164e1d26ca..0000000000
--- a/build/gtest/CMakeFiles/gtest_main.dir/progress.make
+++ /dev/null
@@ -1,2 +0,0 @@
-CMAKE_PROGRESS_1 = 2
-
diff --git a/build/gtest/CMakeFiles/progress.marks b/build/gtest/CMakeFiles/progress.marks
deleted file mode 100644
index 573541ac97..0000000000
--- a/build/gtest/CMakeFiles/progress.marks
+++ /dev/null
@@ -1 +0,0 @@
-0
diff --git a/build/gtest/CTestTestfile.cmake b/build/gtest/CTestTestfile.cmake
deleted file mode 100644
index bd4b57a8bf..0000000000
--- a/build/gtest/CTestTestfile.cmake
+++ /dev/null
@@ -1,6 +0,0 @@
-# CMake generated Testfile for 
-# Source directory: /usr/src/gtest
-# Build directory: /home/roman/src/Firmware/build/gtest
-# 
-# This file includes the relevant testing commands required for 
-# testing this directory and lists subdirectories to be tested as well.
diff --git a/build/gtest/Makefile b/build/gtest/Makefile
deleted file mode 100644
index bc8c23a004..0000000000
--- a/build/gtest/Makefile
+++ /dev/null
@@ -1,262 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 2.8
-
-# Default target executed when no arguments are given to make.
-default_target: all
-.PHONY : default_target
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-# A target that is always out of date.
-cmake_force:
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/roman/src/Firmware/src
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/roman/src/Firmware/build
-
-#=============================================================================
-# Targets provided globally by CMake.
-
-# Special rule for the target edit_cache
-edit_cache:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..."
-	/usr/bin/cmake -i .
-.PHONY : edit_cache
-
-# Special rule for the target edit_cache
-edit_cache/fast: edit_cache
-.PHONY : edit_cache/fast
-
-# Special rule for the target install
-install: preinstall
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
-	/usr/bin/cmake -P cmake_install.cmake
-.PHONY : install
-
-# Special rule for the target install
-install/fast: preinstall/fast
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
-	/usr/bin/cmake -P cmake_install.cmake
-.PHONY : install/fast
-
-# Special rule for the target install/local
-install/local: preinstall
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..."
-	/usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake
-.PHONY : install/local
-
-# Special rule for the target install/local
-install/local/fast: install/local
-.PHONY : install/local/fast
-
-# Special rule for the target install/strip
-install/strip: preinstall
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..."
-	/usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake
-.PHONY : install/strip
-
-# Special rule for the target install/strip
-install/strip/fast: install/strip
-.PHONY : install/strip/fast
-
-# Special rule for the target list_install_components
-list_install_components:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\""
-.PHONY : list_install_components
-
-# Special rule for the target list_install_components
-list_install_components/fast: list_install_components
-.PHONY : list_install_components/fast
-
-# Special rule for the target rebuild_cache
-rebuild_cache:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
-	/usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
-.PHONY : rebuild_cache
-
-# Special rule for the target rebuild_cache
-rebuild_cache/fast: rebuild_cache
-.PHONY : rebuild_cache/fast
-
-# Special rule for the target test
-test:
-	@$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running tests..."
-	/usr/bin/ctest --force-new-ctest-process $(ARGS)
-.PHONY : test
-
-# Special rule for the target test
-test/fast: test
-.PHONY : test/fast
-
-# The main all target
-all: cmake_check_build_system
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles /home/roman/src/Firmware/build/gtest/CMakeFiles/progress.marks
-	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/all
-	$(CMAKE_COMMAND) -E cmake_progress_start /home/roman/src/Firmware/build/CMakeFiles 0
-.PHONY : all
-
-# The main clean target
-clean:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/clean
-.PHONY : clean
-
-# The main clean target
-clean/fast: clean
-.PHONY : clean/fast
-
-# Prepare targets for installation.
-preinstall: all
-	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall
-.PHONY : preinstall
-
-# Prepare targets for installation.
-preinstall/fast:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/preinstall
-.PHONY : preinstall/fast
-
-# clear depends
-depend:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
-.PHONY : depend
-
-# Convenience name for target.
-gtest/CMakeFiles/gtest.dir/rule:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest.dir/rule
-.PHONY : gtest/CMakeFiles/gtest.dir/rule
-
-# Convenience name for target.
-gtest: gtest/CMakeFiles/gtest.dir/rule
-.PHONY : gtest
-
-# fast build rule for target.
-gtest/fast:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/build
-.PHONY : gtest/fast
-
-# Convenience name for target.
-gtest/CMakeFiles/gtest_main.dir/rule:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f CMakeFiles/Makefile2 gtest/CMakeFiles/gtest_main.dir/rule
-.PHONY : gtest/CMakeFiles/gtest_main.dir/rule
-
-# Convenience name for target.
-gtest_main: gtest/CMakeFiles/gtest_main.dir/rule
-.PHONY : gtest_main
-
-# fast build rule for target.
-gtest_main/fast:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/build
-.PHONY : gtest_main/fast
-
-src/gtest-all.o: src/gtest-all.cc.o
-.PHONY : src/gtest-all.o
-
-# target to build an object file
-src/gtest-all.cc.o:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o
-.PHONY : src/gtest-all.cc.o
-
-src/gtest-all.i: src/gtest-all.cc.i
-.PHONY : src/gtest-all.i
-
-# target to preprocess a source file
-src/gtest-all.cc.i:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.i
-.PHONY : src/gtest-all.cc.i
-
-src/gtest-all.s: src/gtest-all.cc.s
-.PHONY : src/gtest-all.s
-
-# target to generate assembly for a file
-src/gtest-all.cc.s:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest.dir/build.make gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.s
-.PHONY : src/gtest-all.cc.s
-
-src/gtest_main.o: src/gtest_main.cc.o
-.PHONY : src/gtest_main.o
-
-# target to build an object file
-src/gtest_main.cc.o:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.o
-.PHONY : src/gtest_main.cc.o
-
-src/gtest_main.i: src/gtest_main.cc.i
-.PHONY : src/gtest_main.i
-
-# target to preprocess a source file
-src/gtest_main.cc.i:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.i
-.PHONY : src/gtest_main.cc.i
-
-src/gtest_main.s: src/gtest_main.cc.s
-.PHONY : src/gtest_main.s
-
-# target to generate assembly for a file
-src/gtest_main.cc.s:
-	cd /home/roman/src/Firmware/build && $(MAKE) -f gtest/CMakeFiles/gtest_main.dir/build.make gtest/CMakeFiles/gtest_main.dir/src/gtest_main.cc.s
-.PHONY : src/gtest_main.cc.s
-
-# Help Target
-help:
-	@echo "The following are some of the valid targets for this Makefile:"
-	@echo "... all (the default if no target is provided)"
-	@echo "... clean"
-	@echo "... depend"
-	@echo "... edit_cache"
-	@echo "... gtest"
-	@echo "... gtest_main"
-	@echo "... install"
-	@echo "... install/local"
-	@echo "... install/strip"
-	@echo "... list_install_components"
-	@echo "... rebuild_cache"
-	@echo "... test"
-	@echo "... src/gtest-all.o"
-	@echo "... src/gtest-all.i"
-	@echo "... src/gtest-all.s"
-	@echo "... src/gtest_main.o"
-	@echo "... src/gtest_main.i"
-	@echo "... src/gtest_main.s"
-.PHONY : help
-
-
-
-#=============================================================================
-# Special targets to cleanup operation of make.
-
-# Special rule to run CMake to check the build system integrity.
-# No rule that depends on this can have commands that come from listfiles
-# because they might be regenerated.
-cmake_check_build_system:
-	cd /home/roman/src/Firmware/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
-.PHONY : cmake_check_build_system
-
diff --git a/build/gtest/cmake_install.cmake b/build/gtest/cmake_install.cmake
deleted file mode 100644
index f51b656514..0000000000
--- a/build/gtest/cmake_install.cmake
+++ /dev/null
@@ -1,34 +0,0 @@
-# Install script for directory: /usr/src/gtest
-
-# Set the install prefix
-IF(NOT DEFINED CMAKE_INSTALL_PREFIX)
-  SET(CMAKE_INSTALL_PREFIX "/home/roman/src/Firmware/install")
-ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX)
-STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
-# Set the install configuration name.
-IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
-  IF(BUILD_TYPE)
-    STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
-           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
-  ELSE(BUILD_TYPE)
-    SET(CMAKE_INSTALL_CONFIG_NAME "")
-  ENDIF(BUILD_TYPE)
-  MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
-ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
-
-# Set the component getting installed.
-IF(NOT CMAKE_INSTALL_COMPONENT)
-  IF(COMPONENT)
-    MESSAGE(STATUS "Install component: \"${COMPONENT}\"")
-    SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
-  ELSE(COMPONENT)
-    SET(CMAKE_INSTALL_COMPONENT)
-  ENDIF(COMPONENT)
-ENDIF(NOT CMAKE_INSTALL_COMPONENT)
-
-# Install shared libraries without execute permission?
-IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
-  SET(CMAKE_INSTALL_SO_NO_EXE "1")
-ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
-

From 7954540f45b662ade0ad4038cf936934d3d1eef7 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 17:52:43 +0100
Subject: [PATCH 041/416] removed files which should not be in here

---
 ROMFS/px4fmu_common/init.d/13000_quadshot     |  14 -
 .../init.d/Roman_mavlink_stream_conf          |  12 -
 ROMFS/px4fmu_common/init.d/rc.vtol_apps       |  15 -
 ROMFS/px4fmu_common/init.d/rc.vtol_defaults   |  63 --
 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix   |  15 -
 src/modules/vtol_att_control/module.mk        |  40 --
 .../vtol_att_control_main.cpp                 | 642 ------------------
 7 files changed, 801 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/init.d/13000_quadshot
 delete mode 100644 ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf
 delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps
 delete mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults
 delete mode 100644 ROMFS/px4fmu_common/mixers/FMU_quadshot.mix
 delete mode 100644 src/modules/vtol_att_control/module.mk
 delete mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp

diff --git a/ROMFS/px4fmu_common/init.d/13000_quadshot b/ROMFS/px4fmu_common/init.d/13000_quadshot
deleted file mode 100644
index 8ee306a387..0000000000
--- a/ROMFS/px4fmu_common/init.d/13000_quadshot
+++ /dev/null
@@ -1,14 +0,0 @@
-#!nsh
-#
-# Generic quadshot configuration file
-#
-# Roman Bapst <romanbapst@yahoo.de>
-#
-
-sh /etc/init.d/rc.mc_defaults
-
-set MIXER FMU_quadshot
-
-set PWM_OUTPUTS 1234
-set PWM_MIN 1070
-set PWM_MAX 2000
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf b/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf
deleted file mode 100644
index d26e4a3722..0000000000
--- a/ROMFS/px4fmu_common/init.d/Roman_mavlink_stream_conf
+++ /dev/null
@@ -1,12 +0,0 @@
-#!nsh
-# Configure stream for Mavlink instance on TELEM2 because it is annoying always removing the SDcard
-#
-#usleep 100000
-#mavlink stream -d /dev/ttyS2 -s ATTITUDE_CONTROLS -r 50
-#usleep 100000
-#mavlink stream -d /dev/ttyS2 -s RC_CHANNELS_RAW -r 50
-#usleep 100000
-#mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 50
-usleep 100000
-mavlink stream -d /dev/ttyS2 -s MANUAL_CONTROL -r 50
-echo "Added additional streams on TELEM2"
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
deleted file mode 100644
index 23ade6d78b..0000000000
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps
+++ /dev/null
@@ -1,15 +0,0 @@
-#!nsh
-#
-# Standard apps for vtol:
-# att & pos estimator, att & pos control.
-#
-
-attitude_estimator_ekf start
-#ekf_att_pos_estimator start
-position_estimator_inav start
-
-vtol_att_control start
-mc_att_control start
-mc_pos_control start
-fw_att_control start
-fw_pos_control_l1 start
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
deleted file mode 100644
index f0ea9add07..0000000000
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+++ /dev/null
@@ -1,63 +0,0 @@
-#!nsh
-
-set VEHICLE_TYPE vtol
-
-if [ $DO_AUTOCONFIG == yes ]
-then
-	#Default parameters for MC
-	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.1
-	param set MC_ROLLRATE_I 0.0
-	param set MC_ROLLRATE_D 0.003
-	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.1
-	param set MC_PITCHRATE_I 0.0
-	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 MC_YAW_FF 0.5
-
-	param set MPC_THR_MAX 1.0
-	param set MPC_THR_MIN 0.1
-	param set MPC_XY_P 1.0
-	param set MPC_XY_VEL_P 0.1
-	param set MPC_XY_VEL_I 0.02
-	param set MPC_XY_VEL_D 0.01
-	param set MPC_XY_VEL_MAX 5
-	param set MPC_XY_FF 0.5
-	param set MPC_Z_P 1.0
-	param set MPC_Z_VEL_P 0.1
-	param set MPC_Z_VEL_I 0.02
-	param set MPC_Z_VEL_D 0.0
-	param set MPC_Z_VEL_MAX 3
-	param set MPC_Z_FF 0.5
-	param set MPC_TILTMAX_AIR 45.0
-	param set MPC_TILTMAX_LND 15.0
-	param set MPC_LAND_SPEED 1.0
-
-	param set PE_VELNE_NOISE 0.5
-	param set PE_VELD_NOISE 0.7
-	param set PE_POSNE_NOISE 0.5
-	param set PE_POSD_NOISE 1.0
-
-	param set NAV_ACCEPT_RAD 2.0
-	
-	#
-	# Default parameters for FW
-	#
-	param set NAV_LAND_ALT 90
-	param set NAV_RTL_ALT 100
-	param set NAV_RTL_LAND_T -1
-	param set NAV_ACCEPT_RAD 50
-	param set FW_T_HRATE_P 0.01
-	param set FW_T_RLL2THR 15
-	param set FW_T_SRATE_P 0.01
-	param set FW_T_TIME_CONST 5
-fi
-
-set PWM_RATE 400
-set PWM_DISARMED 900
-set PWM_MIN 1075
-set PWM_MAX 2000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix b/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix
deleted file mode 100644
index b077ff30ac..0000000000
--- a/ROMFS/px4fmu_common/mixers/FMU_quadshot.mix
+++ /dev/null
@@ -1,15 +0,0 @@
-#!nsh
-#Quadshot mixer for PX4FMU
-#===========================
-R: 4v 10000 10000 10000 0
-
-#mixer for the elevons
-M: 2
-O:      10000  10000      0 -10000  10000
-S: 1 0   7500   7500      0 -10000  10000
-S: 1 1   8000   8000      0 -10000  10000
-
-M: 2
-O:      10000  10000      0 -10000  10000
-S: 1 0   7500   7500      0 -10000  10000
-S: 1 1  -8000  -8000      0 -10000  10000
diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk
deleted file mode 100644
index c349c23401..0000000000
--- a/src/modules/vtol_att_control/module.mk
+++ /dev/null
@@ -1,40 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# VTOL attitude controller
-#
-
-MODULE_COMMAND	= vtol_att_control
-
-SRCS		= vtol_att_control_main.cpp
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
deleted file mode 100644
index 38fa4eec18..0000000000
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ /dev/null
@@ -1,642 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file VTOL_att_control_main.cpp
- * Implementation of an attitude controller for VTOL airframes. This module receives data
- * from both the fixed wing- and the multicopter attitude controllers and processes it.
- * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
- * flight or transition). It also publishes the resulting controls on the actuator controls topics.
- *
- * @author Roman Bapst 		<bapstr@ethz.ch>
- * @author Lorenz Meier 	<lm@inf.ethz.ch>
- * @author Thomas Gubler	<thomasgubler@gmail.com>
- *
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.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/vehicle_control_mode.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/circuit_breaker.h>
-#include <lib/mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-
-#include "drivers/drv_pwm_output.h"
-#include <nuttx/fs/nxffs.h>
-#include <nuttx/fs/ioctl.h>
-
-#include <nuttx/mtd.h>
-
-#include <fcntl.h>
-
-
-extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
-
-class VtolAttitudeControl
-{
-public:
-
-	VtolAttitudeControl();
-	~VtolAttitudeControl();
-
-	int start();	/* start the task and return OK on success */
-
-
-private:
-//******************flags & handlers******************************************************
-	bool _task_should_exit;
-	int _control_task;		//task handle for VTOL attitude controller
-
-	/* handlers for subscriptions */
-	int		_v_att_sub;				//vehicle attitude subscription
-	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
-	int		_v_rates_sp_sub;		//vehicle rates setpoint subscription
-	int		_v_control_mode_sub;	//vehicle control mode subscription
-	int		_params_sub;			//parameter updates subscription
-	int		_manual_control_sp_sub;	//manual control setpoint subscription
-	int		_armed_sub;				//arming status subscription
-
-	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
-	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
-
-	//handlers for publishers
-	orb_advert_t	_actuators_0_pub;		//input for the mixer (roll,pitch,yaw,thrust)
-	orb_advert_t 	_actuators_1_pub;
-	orb_advert_t	_vtol_vehicle_status_pub;
-//*******************data containers***********************************************************
-	struct vehicle_attitude_s			_v_att;				//vehicle attitude
-	struct vehicle_attitude_setpoint_s	_v_att_sp;			//vehicle attitude setpoint
-	struct vehicle_rates_setpoint_s		_v_rates_sp;		//vehicle rates setpoint
-	struct manual_control_setpoint_s	_manual_control_sp; //manual control setpoint
-	struct vehicle_control_mode_s		_v_control_mode;	//vehicle control mode
-	struct vtol_vehicle_status_s 		_vtol_vehicle_status;
-	struct actuator_controls_s			_actuators_out_0;	//actuator controls going to the mc mixer
-	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
-	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
-	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
-	struct actuator_armed_s				_armed;				//actuator arming status
-
-	struct {
-		float min_pwm_mc;	//pwm value for idle in mc mode
-	} _params;
-
-	struct {
-		param_t min_pwm_mc;
-	} _params_handles;
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	/* for multicopters it is usual to have a non-zero idle speed of the engines
-	 * for fixed wings we want to have an idle speed of zero since we do not want
-	 * to waste energy when gliding. */
-	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
-
-//*****************Member functions***********************************************************************
-
-	void 		task_main();	//main task
-	static void	task_main_trampoline(int argc, char *argv[]);	//Shim for calling task_main from task_create.
-
-	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
-	void		vehicle_manual_poll();			//Check for changes in manual inputs.
-	void		arming_status_poll();			//Check for arming status updates.
-	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
-	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
-	void 		parameters_update_poll();		//Check if parameters have changed
-	int 		parameters_update();			//Update local paraemter cache
-	void  		fill_mc_att_control_output();	//write mc_att_control results to actuator message
-	void		fill_fw_att_control_output();	//write fw_att_control results to actuator message
-	void 		set_idle_fw();
-	void 		set_idle_mc();
-};
-
-namespace VTOL_att_control
-{
-VtolAttitudeControl *g_control;
-}
-
-/**
-* Constructor
-*/
-VtolAttitudeControl::VtolAttitudeControl() :
-	_task_should_exit(false),
-	_control_task(-1),
-
-	//init subscription handlers
-	_v_att_sub(-1),
-	_v_att_sp_sub(-1),
-	_v_control_mode_sub(-1),
-	_params_sub(-1),
-	_manual_control_sp_sub(-1),
-	_armed_sub(-1),
-
-	//init publication handlers
-	_actuators_0_pub(-1),
-	_actuators_1_pub(-1),
-	_vtol_vehicle_status_pub(-1),
-
-	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
-{
-
-	flag_idle_mc = true;	/*assume we always start in mc mode for a VTOL airframe */
-
-	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
-	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
-	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
-	memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
-	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
-	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
-	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
-	memset(&_armed, 0, sizeof(_armed));
-
-	_params_handles.min_pwm_mc = param_find("PWM_MIN");
-
-	/* fetch initial parameter values */
-	parameters_update();
-}
-
-/**
-* Destructor
-*/
-VtolAttitudeControl::~VtolAttitudeControl()
-{
-	if (_control_task != -1) {
-		/* task wakes up every 100ms or so at the longest */
-		_task_should_exit = true;
-
-		/* wait for a second for the task to quit at our request */
-		unsigned i = 0;
-
-		do {
-			/* wait 20ms */
-			usleep(20000);
-
-			/* if we have given up, kill it */
-			if (++i > 50) {
-				task_delete(_control_task);
-				break;
-			}
-		} while (_control_task != -1);
-	}
-
-	VTOL_att_control::g_control = nullptr;
-}
-
-/**
-* Check for changes in vehicle control mode.
-*/
-void VtolAttitudeControl::vehicle_control_mode_poll()
-{
-	bool updated;
-
-	/* Check if vehicle control mode has changed */
-	orb_check(_v_control_mode_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
-	}
-}
-
-/**
-* Check for changes in manual inputs.
-*/
-void VtolAttitudeControl::vehicle_manual_poll()
-{
-	bool updated;
-
-	/* get pilots inputs */
-	orb_check(_manual_control_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
-	}
-}
-/**
-* Check for arming status updates.
-*/
-void VtolAttitudeControl::arming_status_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_armed_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-	}
-}
-
-/**
-* Check for inputs from mc attitude controller.
-*/
-void VtolAttitudeControl::actuator_controls_mc_poll()
-{
-	bool updated;
-	orb_check(_actuator_inputs_mc, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in);
-	}
-}
-
-/**
-* Check for inputs from fw attitude controller.
-*/
-void VtolAttitudeControl::actuator_controls_fw_poll()
-{
-	bool updated;
-	orb_check(_actuator_inputs_fw, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in);
-	}
-}
-
-/**
-* Check for parameter updates.
-*/
-void
-VtolAttitudeControl::parameters_update_poll()
-{
-	bool updated;
-
-	/* Check if parameters have changed */
-	orb_check(_params_sub, &updated);
-
-	if (updated) {
-		struct parameter_update_s param_update;
-		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
-		parameters_update();
-	}
-}
-
-/**
-* Update parameters.
-*/
-int
-VtolAttitudeControl::parameters_update()
-{
-	/* idle pwm */
-	float v;
-	param_get(_params_handles.min_pwm_mc, &v);
-	_params.min_pwm_mc = v;
-
-	return OK;
-}
-
-/**
-* Prepare message to acutators with data from mc attitude controller.
-*/
-void VtolAttitudeControl::fill_mc_att_control_output()
-{
-	_actuators_out_0.control[0] = _actuators_mc_in.control[0];
-	_actuators_out_0.control[1] = _actuators_mc_in.control[1];
-	_actuators_out_0.control[2] = _actuators_mc_in.control[2];
-	_actuators_out_0.control[3] = _actuators_mc_in.control[3];
-	//set neutral position for elevons
-	_actuators_out_1.control[0] = 0;	//roll elevon
-	_actuators_out_1.control[1] = 0;	//pitch elevon
-}
-
-/**
-* Prepare message to acutators with data from fw attitude controller.
-*/
-void VtolAttitudeControl::fill_fw_att_control_output()
-{
-	/*For the first test in fw mode, only use engines for thrust!!!*/
-	_actuators_out_0.control[0] = 0;
-	_actuators_out_0.control[1] = 0;
-	_actuators_out_0.control[2] = 0;
-	_actuators_out_0.control[3] = _actuators_fw_in.control[3];
-	/*controls for the elevons */
-	_actuators_out_1.control[0] = _actuators_fw_in.control[0];	/*roll elevon*/
-	_actuators_out_1.control[1] = _actuators_fw_in.control[1];	/*pitch elevon */
-}
-
-/**
-* Adjust idle speed for fw mode.
-*/
-void VtolAttitudeControl::set_idle_fw()
-{
-	int ret;
-	char *dev = PWM_OUTPUT_DEVICE_PATH;
-	int fd = open(dev, 0);
-
-	if (fd < 0) {err(1, "can't open %s", dev);}
-
-	unsigned pwm_value = PWM_LOWEST_MIN;
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
-
-	for (unsigned i = 0; i < 4; i++) {
-
-		pwm_values.values[i] = pwm_value;
-		pwm_values.channel_count++;
-	}
-
-	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
-
-	if (ret != OK) {errx(ret, "failed setting min values");}
-
-	close(fd);
-}
-
-/**
-* Adjust idle speed for mc mode.
-*/
-void VtolAttitudeControl::set_idle_mc()
-{
-	int ret;
-	unsigned servo_count;
-	char *dev = PWM_OUTPUT_DEVICE_PATH;
-	int fd = open(dev, 0);
-
-	if (fd < 0) {err(1, "can't open %s", dev);}
-
-	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
-	unsigned pwm_value = 1100;
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
-
-	for (unsigned i = 0; i < 4; i++) {
-		pwm_values.values[i] = pwm_value;
-		pwm_values.channel_count++;
-	}
-
-	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
-
-	if (ret != OK) {errx(ret, "failed setting min values");}
-
-	close(fd);
-}
-
-void
-VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
-{
-	VTOL_att_control::g_control->task_main();
-}
-
-void VtolAttitudeControl::task_main()
-{
-	warnx("started");
-	fflush(stdout);
-
-	/* do subscriptions */
-	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	_v_rates_sp_sub        = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
-	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
-
-	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
-	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
-
-	parameters_update();  /*initialize parameter cache/*
-
-	/* wakeup source*/
-	struct pollfd fds[3];	/*input_mc, input_fw, parameters*/
-
-	fds[0].fd     = _actuator_inputs_mc;
-	fds[0].events = POLLIN;
-	fds[1].fd     = _actuator_inputs_fw;
-	fds[1].events = POLLIN;
-	fds[2].fd     = _params_sub;
-	fds[2].events = POLLIN;
-
-	while (!_task_should_exit) {
-		/*Advertise/Publish vtol vehicle status*/
-		if (_vtol_vehicle_status_pub > 0) {
-			orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
-
-		} else {
-			_vtol_vehicle_status.timestamp = hrt_absolute_time();
-			_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
-		}
-
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
-
-
-		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0) {
-			continue;
-		}
-
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
-			warn("poll error %d, %d", pret, errno);
-			/* sleep a bit before next try */
-			usleep(100000);
-			continue;
-		}
-
-		if (fds[2].revents & POLLIN) {	//parameters were updated, read them now
-			/* read from param to clear updated flag */
-			struct parameter_update_s update;
-			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
-
-			/* update parameters from storage */
-			parameters_update();
-		}
-
-		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
-		vehicle_manual_poll();			//Check for changes in manual inputs.
-		arming_status_poll();			//Check for arming status updates.
-		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
-		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
-		parameters_update_poll();
-
-		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
-			_vtol_vehicle_status.vtol_in_rw_mode = true;
-
-			if (!flag_idle_mc) {	/* we want to adjust idle speed for mc mode */
-				set_idle_mc();
-				flag_idle_mc = true;
-			}
-
-			/* got data from mc_att_controller */
-			if (fds[0].revents & POLLIN) {
-				vehicle_manual_poll();	/* update remote input */
-				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
-
-				fill_mc_att_control_output();
-
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
-
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
-				}
-
-				if (_actuators_1_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
-
-				} else {
-					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
-				}
-			}
-		}
-
-		if (_manual_control_sp.aux1 >= 0.0f) {			/* vehicle is in fw mode */
-			_vtol_vehicle_status.vtol_in_rw_mode = false;
-
-			if (flag_idle_mc) {	/* we want to adjust idle speed for fixed wing mode */
-				set_idle_fw();
-				flag_idle_mc = false;
-			}
-
-			if (fds[1].revents & POLLIN) {		/* got data from fw_att_controller */
-				orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
-				vehicle_manual_poll();	//update remote input
-
-				fill_fw_att_control_output();
-
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
-
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
-				}
-
-				if (_actuators_1_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
-
-				} else {
-					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
-				}
-			}
-		}
-	}
-
-	warnx("exit");
-	_control_task = -1;
-	_exit(0);
-}
-
-int
-VtolAttitudeControl::start()
-{
-	ASSERT(_control_task == -1);
-
-	/* start the task */
-	_control_task = task_spawn_cmd("vtol_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 10,
-				       2048,
-				       (main_t)&VtolAttitudeControl::task_main_trampoline,
-				       nullptr);
-
-	if (_control_task < 0) {
-		warn("task start failed");
-		return -errno;
-	}
-
-	return OK;
-}
-
-
-int vtol_att_control_main(int argc, char *argv[])
-{
-	if (argc < 1) {
-		errx(1, "usage: vtol_att_control {start|stop|status}");
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (VTOL_att_control::g_control != nullptr) {
-			errx(1, "already running");
-		}
-
-		VTOL_att_control::g_control = new VtolAttitudeControl;
-
-		if (VTOL_att_control::g_control == nullptr) {
-			errx(1, "alloc failed");
-		}
-
-		if (OK != VTOL_att_control::g_control->start()) {
-			delete VTOL_att_control::g_control;
-			VTOL_att_control::g_control = nullptr;
-			err(1, "start failed");
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		if (VTOL_att_control::g_control == nullptr) {
-			errx(1, "not running");
-		}
-
-		delete VTOL_att_control::g_control;
-		VTOL_att_control::g_control = nullptr;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (VTOL_att_control::g_control) {
-			errx(0, "running");
-
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	return 1;
-}

From 4915c15036604b56f0d1fa4a42a44c9e6cacbe64 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 10 Nov 2014 17:58:33 +0100
Subject: [PATCH 042/416] removed files and code segments which should not be
 there and removed

---
 ROMFS/px4fmu_common/init.d/rc.autostart       |  9 ---
 ROMFS/px4fmu_common/init.d/rcS                |  1 -
 src/modules/uORB/topics/vtol_vehicle_status.h | 66 -------------------
 3 files changed, 76 deletions(-)
 delete mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h

diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 01aa8ed13b..78778d8066 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -16,7 +16,6 @@
 # 10000	..	10999		Wide arm / H frame
 # 11000	..	11999		Hexa Cox
 # 12000	..	12999		Octo Cox
-# 13000 ..  13999		Vtol
 
 #
 # Simulation setups
@@ -239,11 +238,3 @@ if param compare SYS_AUTOSTART 12001
 then
 	sh /etc/init.d/12001_octo_cox
 fi
-
-#
-# Quadshot
-#
- if param compare SYS_AUTOSTART 13000
- then
- 	 sh /etc/init.d/13000_quadshot
- fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 39172eea4c..ea04ece34d 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -589,7 +589,6 @@ then
 	then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
-		sh /etc/init.d/Roman_mavlink_stream_conf
 	else
 		echo "[init] No addons script: $EXTRAS_FILE"
 	fi
diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h
deleted file mode 100644
index 24ecca9faa..0000000000
--- a/src/modules/uORB/topics/vtol_vehicle_status.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vtol_status.h
- *
- * Vtol status topic
- *
- */
-
-#ifndef TOPIC_VTOL_STATUS_H
-#define TOPIC_VTOL_STATUS_H
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/* Indicates in which mode the vtol aircraft is in */
-struct vtol_vehicle_status_s {
-
-	uint64_t	timestamp;	/**< Microseconds since system boot */
-	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vtol_vehicle_status);
-
-#endif

From 7e350555ea634013f083bccc2f5a8108fc5ef9f9 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 12:36:05 +0100
Subject: [PATCH 043/416] checked out from PX4 master to avoid eclipse
 formatting which happened in the past

---
 .../fw_att_control/fw_att_control_main.cpp    | 398 +++++++++---------
 1 file changed, 195 insertions(+), 203 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 80b58ec714..e770c11a27 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -83,7 +83,8 @@
  */
 extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
 
-class FixedwingAttitudeControl {
+class FixedwingAttitudeControl
+{
 public:
 	/**
 	 * Constructor
@@ -100,56 +101,54 @@ public:
 	 *
 	 * @return	OK on success.
 	 */
-	int start();
+	int		start();
 
 	/**
 	 * Task status
 	 *
 	 * @return	true if the mainloop is running
 	 */
-	bool task_running() {
-		return _task_running;
-	}
+	bool		task_running() { return _task_running; }
 
 private:
 
-	bool _task_should_exit; /**< if true, sensor task should exit */
-	bool _task_running; /**< if true, task is running in its mainloop */
-	int _control_task; /**< task handle for sensor task */
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
+	int		_control_task;			/**< task handle for sensor task */
 
-	int _att_sub; /**< vehicle attitude 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 */
-	int _global_pos_sub; /**< global position subscription */
-	int _vehicle_status_sub; /**< vehicle status subscription */
+	int		_att_sub;			/**< vehicle attitude 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 */
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_vehicle_status_sub;		/**< vehicle status subscription */
 
-	orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
-	orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
-	orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
-	orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
+	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
+	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
+	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
+	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
-	struct vehicle_attitude_s _att; /**< vehicle attitude */
-	struct accel_report _accel; /**< body frame accelerations */
-	struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude 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 */
-	struct vehicle_global_position_s _global_pos; /**< global position */
-	struct vehicle_status_s _vehicle_status; /**< vehicle status */
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude 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 */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
 
-	perf_counter_t _loop_perf; /**< loop performance counter */
-	perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
-	perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
 
-	bool _setpoint_valid; /**< flag if the position control setpoint is valid */
-	bool _debug; /**< if set to true, print debug output */
+	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_debug;				/**< if set to true, print debug output */
 
 	struct {
 		float tconst;
@@ -183,14 +182,14 @@ private:
 		float trim_roll;
 		float trim_pitch;
 		float trim_yaw;
-		float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
-		float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
-		float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
-		float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
-		float man_roll_max; /**< Max Roll in rad */
-		float man_pitch_max; /**< Max Pitch in rad */
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */
 
-	} _parameters; /**< local copies of interesting parameters */
+	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
 
@@ -229,71 +228,75 @@ private:
 		param_t pitchsp_offset_deg;
 		param_t man_roll_max;
 		param_t man_pitch_max;
-	} _parameter_handles; /**< handles for interesting parameters */
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+
+	ECL_RollController				_roll_ctrl;
+	ECL_PitchController				_pitch_ctrl;
+	ECL_YawController				_yaw_ctrl;
 
-	ECL_RollController _roll_ctrl;
-	ECL_PitchController _pitch_ctrl;
-	ECL_YawController _yaw_ctrl;
 
 	/**
 	 * Update our local parameter cache.
 	 */
-	int parameters_update();
+	int		parameters_update();
 
 	/**
 	 * Update control outputs
 	 *
 	 */
-	void control_update();
+	void		control_update();
 
 	/**
 	 * Check for changes in vehicle control mode.
 	 */
-	void vehicle_control_mode_poll();
+	void		vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in manual inputs.
 	 */
-	void vehicle_manual_poll();
+	void		vehicle_manual_poll();
+
 
 	/**
 	 * Check for airspeed updates.
 	 */
-	void vehicle_airspeed_poll();
+	void		vehicle_airspeed_poll();
 
 	/**
 	 * Check for accel updates.
 	 */
-	void vehicle_accel_poll();
+	void		vehicle_accel_poll();
 
 	/**
 	 * Check for set triplet updates.
 	 */
-	void vehicle_setpoint_poll();
+	void		vehicle_setpoint_poll();
 
 	/**
 	 * Check for global position updates.
 	 */
-	void global_pos_poll();
+	void		global_pos_poll();
 
 	/**
 	 * Check for vehicle status updates.
 	 */
-	void vehicle_status_poll();
+	void		vehicle_status_poll();
 
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
-	static void task_main_trampoline(int argc, char *argv[]);
+	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
 	 * Main sensor collection task.
 	 */
-	void task_main();
+	void		task_main();
 
 };
 
-namespace att_control {
+namespace att_control
+{
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -301,28 +304,39 @@ namespace att_control {
 #endif
 static const int ERROR = -1;
 
-FixedwingAttitudeControl *g_control = nullptr;
+FixedwingAttitudeControl	*g_control = nullptr;
 }
 
 FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
-		_task_should_exit(false), _task_running(false), _control_task(-1),
+	_task_should_exit(false),
+	_task_running(false),
+	_control_task(-1),
 
-		/* subscriptions */
-		_att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(
-				-1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub(
-				-1),
+/* subscriptions */
+	_att_sub(-1),
+	_accel_sub(-1),
+	_airspeed_sub(-1),
+	_vcontrol_mode_sub(-1),
+	_params_sub(-1),
+	_manual_sub(-1),
+	_global_pos_sub(-1),
+	_vehicle_status_sub(-1),
 
-		/* publications */
-		_rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub(
-				-1),
+/* publications */
+	_rate_sp_pub(-1),
+	_attitude_sp_pub(-1),
+	_actuators_0_pub(-1),
+	_actuators_1_pub(-1),
 
-		/* performance counters */
-		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
-				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
-				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
-		/* states */
-		_setpoint_valid(false), _debug(false) {
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
+	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+/* states */
+	_setpoint_valid(false),
+	_debug(false)
+{
 	/* safely initialize structs */
 	_att = {};
 	_accel = {};
@@ -335,6 +349,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_global_pos = {};
 	_vehicle_status = {};
 
+
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
 	_parameter_handles.p_p = param_find("FW_PR_P");
 	_parameter_handles.p_i = param_find("FW_PR_I");
@@ -375,7 +390,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	parameters_update();
 }
 
-FixedwingAttitudeControl::~FixedwingAttitudeControl() {
+FixedwingAttitudeControl::~FixedwingAttitudeControl()
+{
 	if (_control_task != -1) {
 
 		/* task wakes up every 100ms or so at the longest */
@@ -403,7 +419,9 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() {
 	att_control::g_control = nullptr;
 }
 
-int FixedwingAttitudeControl::parameters_update() {
+int
+FixedwingAttitudeControl::parameters_update()
+{
 
 	param_get(_parameter_handles.tconst, &(_parameters.tconst));
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
@@ -411,26 +429,21 @@ int FixedwingAttitudeControl::parameters_update() {
 	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
-	param_get(_parameter_handles.p_integrator_max,
-			&(_parameters.p_integrator_max));
-	param_get(_parameter_handles.p_roll_feedforward,
-			&(_parameters.p_roll_feedforward));
+	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
+	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
 	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
 
-	param_get(_parameter_handles.r_integrator_max,
-			&(_parameters.r_integrator_max));
+	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
 	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
-	param_get(_parameter_handles.y_integrator_max,
-			&(_parameters.y_integrator_max));
-	param_get(_parameter_handles.y_coordinated_min_speed,
-			&(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
@@ -440,19 +453,16 @@ int FixedwingAttitudeControl::parameters_update() {
 	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
 	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
 	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
-	param_get(_parameter_handles.rollsp_offset_deg,
-			&(_parameters.rollsp_offset_deg));
-	param_get(_parameter_handles.pitchsp_offset_deg,
-			&(_parameters.pitchsp_offset_deg));
-	_parameters.rollsp_offset_rad = math::radians(
-			_parameters.rollsp_offset_deg);
-	_parameters.pitchsp_offset_rad = math::radians(
-			_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
 	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
 	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
+
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -482,7 +492,9 @@ int FixedwingAttitudeControl::parameters_update() {
 	return OK;
 }
 
-void FixedwingAttitudeControl::vehicle_control_mode_poll() {
+void
+FixedwingAttitudeControl::vehicle_control_mode_poll()
+{
 	bool vcontrol_mode_updated;
 
 	/* Check HIL state if vehicle status has changed */
@@ -490,12 +502,13 @@ void FixedwingAttitudeControl::vehicle_control_mode_poll() {
 
 	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub,
-				&_vcontrol_mode);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
 	}
 }
 
-void FixedwingAttitudeControl::vehicle_manual_poll() {
+void
+FixedwingAttitudeControl::vehicle_manual_poll()
+{
 	bool manual_updated;
 
 	/* get pilots inputs */
@@ -507,7 +520,9 @@ void FixedwingAttitudeControl::vehicle_manual_poll() {
 	}
 }
 
-void FixedwingAttitudeControl::vehicle_airspeed_poll() {
+void
+FixedwingAttitudeControl::vehicle_airspeed_poll()
+{
 	/* check if there is a new position */
 	bool airspeed_updated;
 	orb_check(_airspeed_sub, &airspeed_updated);
@@ -518,7 +533,9 @@ void FixedwingAttitudeControl::vehicle_airspeed_poll() {
 	}
 }
 
-void FixedwingAttitudeControl::vehicle_accel_poll() {
+void
+FixedwingAttitudeControl::vehicle_accel_poll()
+{
 	/* check if there is a new position */
 	bool accel_updated;
 	orb_check(_accel_sub, &accel_updated);
@@ -528,7 +545,9 @@ void FixedwingAttitudeControl::vehicle_accel_poll() {
 	}
 }
 
-void FixedwingAttitudeControl::vehicle_setpoint_poll() {
+void
+FixedwingAttitudeControl::vehicle_setpoint_poll()
+{
 	/* check if there is a new setpoint */
 	bool att_sp_updated;
 	orb_check(_att_sp_sub, &att_sp_updated);
@@ -539,18 +558,21 @@ void FixedwingAttitudeControl::vehicle_setpoint_poll() {
 	}
 }
 
-void FixedwingAttitudeControl::global_pos_poll() {
+void
+FixedwingAttitudeControl::global_pos_poll()
+{
 	/* check if there is a new global position */
 	bool global_pos_updated;
 	orb_check(_global_pos_sub, &global_pos_updated);
 
 	if (global_pos_updated) {
-		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub,
-				&_global_pos);
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 	}
 }
 
-void FixedwingAttitudeControl::vehicle_status_poll() {
+void
+FixedwingAttitudeControl::vehicle_status_poll()
+{
 	/* check if there is new status information */
 	bool vehicle_status_updated;
 	orb_check(_vehicle_status_sub, &vehicle_status_updated);
@@ -560,11 +582,15 @@ void FixedwingAttitudeControl::vehicle_status_poll() {
 	}
 }
 
-void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) {
+void
+FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
 	att_control::g_control->task_main();
 }
 
-void FixedwingAttitudeControl::task_main() {
+void
+FixedwingAttitudeControl::task_main()
+{
 
 	/* inform about start */
 	warnx("Initializing..");
@@ -641,6 +667,7 @@ void FixedwingAttitudeControl::task_main() {
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
 
+
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
@@ -694,8 +721,8 @@ void FixedwingAttitudeControl::task_main() {
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */
-				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s)
-						|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
+				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 					if (nonfinite) {
 						perf_count(_nonfinite_input_perf);
@@ -713,9 +740,7 @@ void FixedwingAttitudeControl::task_main() {
 				 * Forcing the scaling to this value allows reasonable handheld tests.
 				 */
 
-				float airspeed_scaling = _parameters.airspeed_trim
-						/ ((airspeed < _parameters.airspeed_min) ?
-								_parameters.airspeed_min : airspeed);
+				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
@@ -731,8 +756,7 @@ void FixedwingAttitudeControl::task_main() {
 						!_vcontrol_mode.flag_control_manual_enabled) {
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-					pitch_sp = _att_sp.pitch_body
-							+ _parameters.pitchsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -758,12 +782,10 @@ void FixedwingAttitudeControl::task_main() {
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.y * _parameters.man_roll_max
-							- _parameters.trim_roll)
-							+ _parameters.rollsp_offset_rad;
-					pitch_sp = -(_manual.x * _parameters.man_pitch_max
-							- _parameters.trim_pitch)
-							+ _parameters.pitchsp_offset_rad;
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+						+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+						+ _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -782,13 +804,11 @@ void FixedwingAttitudeControl::task_main() {
 					/* lazily publish the setpoint only once available */
 					if (_attitude_sp_pub > 0) {
 						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint),
-								_attitude_sp_pub, &att_sp);
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
 
 					} else {
 						/* advertise and publish */
-						_attitude_sp_pub = orb_advertise(
-								ORB_ID(vehicle_attitude_setpoint), &att_sp);
+						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					}
 				}
 
@@ -803,17 +823,11 @@ void FixedwingAttitudeControl::task_main() {
 				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 = _att.R[0][0] * _global_pos.vel_n
-							+ _att.R[1][0] * _global_pos.vel_e
-							+ _att.R[2][0] * _global_pos.vel_d;
-					speed_body_v = _att.R[0][1] * _global_pos.vel_n
-							+ _att.R[1][1] * _global_pos.vel_e
-							+ _att.R[2][1] * _global_pos.vel_d;
-					speed_body_w = _att.R[0][2] * _global_pos.vel_n
-							+ _att.R[1][2] * _global_pos.vel_e
-							+ _att.R[2][2] * _global_pos.vel_d;
-				} else {
+				if(_att.R_valid) 	{
+					speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
+					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
+					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
+				} else	{
 					if (_debug && loop_counter % 10 == 0) {
 						warnx("Did not get a valid R\n");
 					}
@@ -822,81 +836,63 @@ void FixedwingAttitudeControl::task_main() {
 				/* Run attitude controllers */
 				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
 					_roll_ctrl.control_attitude(roll_sp, _att.roll);
-					_pitch_ctrl.control_attitude(pitch_sp, _att.roll,
-							_att.pitch, airspeed);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
 					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
 							speed_body_u, speed_body_v, speed_body_w,
-							_roll_ctrl.get_desired_rate(),
-							_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
 							_att.rollspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max,
-							airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[0] =
-							(isfinite(roll_u)) ?
-									roll_u + _parameters.trim_roll :
-									_parameters.trim_roll;
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						_roll_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("roll_u %.4f", (double) roll_u);
+							warnx("roll_u %.4f", (double)roll_u);
 						}
 					}
 
-					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll,
-							_att.pitch, _att.pitchspeed, _att.yawspeed,
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max,
-							airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[1] =
-							(isfinite(pitch_u)) ?
-									pitch_u + _parameters.trim_pitch :
-									_parameters.trim_pitch;
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
-							warnx(
-									"pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
-											" airspeed %.4f, airspeed_scaling %.4f,"
-											" roll_sp %.4f, pitch_sp %.4f,"
-											" _roll_ctrl.get_desired_rate() %.4f,"
-											" _pitch_ctrl.get_desired_rate() %.4f"
-											" att_sp.roll_body %.4f",
-									(double) pitch_u,
-									(double) _yaw_ctrl.get_desired_rate(),
-									(double) airspeed,
-									(double) airspeed_scaling, (double) roll_sp,
-									(double) pitch_sp,
-									(double) _roll_ctrl.get_desired_rate(),
-									(double) _pitch_ctrl.get_desired_rate(),
-									(double) _att_sp.roll_body);
+							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+								" airspeed %.4f, airspeed_scaling %.4f,"
+								" roll_sp %.4f, pitch_sp %.4f,"
+								" _roll_ctrl.get_desired_rate() %.4f,"
+								" _pitch_ctrl.get_desired_rate() %.4f"
+								" att_sp.roll_body %.4f",
+								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
+								(double)airspeed, (double)airspeed_scaling,
+								(double)roll_sp, (double)pitch_sp,
+								(double)_roll_ctrl.get_desired_rate(),
+								(double)_pitch_ctrl.get_desired_rate(),
+								(double)_att_sp.roll_body);
 						}
 					}
 
-					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll,
-							_att.pitch, _att.pitchspeed, _att.yawspeed,
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
 							_pitch_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max,
-							airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[2] =
-							(isfinite(yaw_u)) ?
-									yaw_u + _parameters.trim_yaw :
-									_parameters.trim_yaw;
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("yaw_u %.4f", (double) yaw_u);
+							warnx("yaw_u %.4f", (double)yaw_u);
 						}
 					}
 
-
 					/* throttle passed through if it is finite and if no engine failure was
 					 * detected */
 					_actuators.control[3] = (isfinite(throttle_sp) &&
@@ -905,15 +901,13 @@ void FixedwingAttitudeControl::task_main() {
 						throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("throttle_sp %.4f", (double) throttle_sp);
+							warnx("throttle_sp %.4f", (double)throttle_sp);
 						}
 					}
 				} else {
 					perf_count(_nonfinite_input_perf);
 					if (_debug && loop_counter % 10 == 0) {
-						warnx(
-								"Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
-								(double) roll_sp, (double) pitch_sp);
+						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
 					}
 				}
 
@@ -930,13 +924,11 @@ void FixedwingAttitudeControl::task_main() {
 
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub,
-							&rates_sp);
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
 
 				} else {
 					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),
-							&rates_sp);
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 				}
 
 			} else {
@@ -958,19 +950,16 @@ void FixedwingAttitudeControl::task_main() {
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub,
-						&_actuators);
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
 
 			} else {
 				/* advertise and publish */
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0),
-						&_actuators);
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
 			}
 
 			if (_actuators_1_pub > 0) {
 				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub,
-						&_actuators_airframe);
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
 //				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
 //						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
 //						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
@@ -978,8 +967,7 @@ void FixedwingAttitudeControl::task_main() {
 
 			} else {
 				/* advertise and publish */
-				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1),
-						&_actuators_airframe);
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
 			}
 
 		}
@@ -995,14 +983,18 @@ void FixedwingAttitudeControl::task_main() {
 	_exit(0);
 }
 
-int FixedwingAttitudeControl::start() {
+int
+FixedwingAttitudeControl::start()
+{
 	ASSERT(_control_task == -1);
 
 	/* start the task */
 	_control_task = task_spawn_cmd("fw_att_control",
-	SCHED_DEFAULT,
-	SCHED_PRIORITY_MAX - 5, 2048,
-			(main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr);
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2048,
+				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
+				       nullptr);
 
 	if (_control_task < 0) {
 		warn("task start failed");
@@ -1012,7 +1004,8 @@ int FixedwingAttitudeControl::start() {
 	return OK;
 }
 
-int fw_att_control_main(int argc, char *argv[]) {
+int fw_att_control_main(int argc, char *argv[])
+{
 	if (argc < 1)
 		errx(1, "usage: fw_att_control {start|stop|status}");
 
@@ -1033,8 +1026,7 @@ int fw_att_control_main(int argc, char *argv[]) {
 		}
 
 		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
-		while (att_control::g_control == nullptr
-				|| !att_control::g_control->task_running()) {
+		while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
 			usleep(50000);
 			printf(".");
 			fflush(stdout);

From 4e0882121e975f19be130c68cacbda3bd817f772 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 13:07:35 +0100
Subject: [PATCH 044/416] removed ifdef because this file is specific to the
 nuttx platform

---
 src/drivers/drv_accel.h | 64 +++++++++++++++++++----------------------
 1 file changed, 30 insertions(+), 34 deletions(-)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 49b104d380..1f98d966bd 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -42,13 +42,12 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
-#ifdef CONFIG_ARCH_ARM
+
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
 #define ACCEL_DEVICE_PATH	"/dev/accel"
 
-#endif
 /**
  * accel report structure.  Reads from the device must be in multiples of this
  * structure.
@@ -82,49 +81,46 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-#ifdef CONFIG_ARCH_ARM
-	ORB_DECLARE(sensor_accel0);
-	ORB_DECLARE(sensor_accel1);
-	ORB_DECLARE(sensor_accel2);
+ORB_DECLARE(sensor_accel0);
+ORB_DECLARE(sensor_accel1);
+ORB_DECLARE(sensor_accel2);
 
-	/*
-	 * ioctl() definitions
-	 *
-	 * Accelerometer drivers also implement the generic sensor driver
-	 * interfaces from drv_sensor.h
-	 */
+/*
+ * ioctl() definitions
+ *
+ * Accelerometer drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
 
-	#define _ACCELIOCBASE		(0x2100)
-	#define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
+#define _ACCELIOCBASE		(0x2100)
+#define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
 
 
-	/** set the accel internal sample rate to at least (arg) Hz */
-	#define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
+/** set the accel internal sample rate to at least (arg) Hz */
+#define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
 
-	/** return the accel internal sample rate in Hz */
-	#define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
+/** return the accel internal sample rate in Hz */
+#define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
 
-	/** set the accel internal lowpass filter to no lower than (arg) Hz */
-	#define ACCELIOCSLOWPASS	_ACCELIOC(2)
+/** set the accel internal lowpass filter to no lower than (arg) Hz */
+#define ACCELIOCSLOWPASS	_ACCELIOC(2)
 
-	/** return the accel internal lowpass filter in Hz */
-	#define ACCELIOCGLOWPASS	_ACCELIOC(3)
+/** return the accel internal lowpass filter in Hz */
+#define ACCELIOCGLOWPASS	_ACCELIOC(3)
 
-	/** set the accel scaling constants to the structure pointed to by (arg) */
-	#define ACCELIOCSSCALE		_ACCELIOC(5)
+/** set the accel scaling constants to the structure pointed to by (arg) */
+#define ACCELIOCSSCALE		_ACCELIOC(5)
 
-	/** get the accel scaling constants into the structure pointed to by (arg) */
-	#define ACCELIOCGSCALE		_ACCELIOC(6)
+/** get the accel scaling constants into the structure pointed to by (arg) */
+#define ACCELIOCGSCALE		_ACCELIOC(6)
 
-	/** set the accel measurement range to handle at least (arg) g */
-	#define ACCELIOCSRANGE		_ACCELIOC(7)
+/** set the accel measurement range to handle at least (arg) g */
+#define ACCELIOCSRANGE		_ACCELIOC(7)
 
-	/** get the current accel measurement range in g */
-	#define ACCELIOCGRANGE		_ACCELIOC(8)
+/** get the current accel measurement range in g */
+#define ACCELIOCGRANGE		_ACCELIOC(8)
 
-	/** get the result of a sensor self-test */
-	#define ACCELIOCSELFTEST	_ACCELIOC(9)
-
-#endif
+/** get the result of a sensor self-test */
+#define ACCELIOCSELFTEST	_ACCELIOC(9)
 
 #endif /* _DRV_ACCEL_H */

From 6b61e725916f90cabd9e8c07144dfcc5ac7b44f6 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 13:08:23 +0100
Subject: [PATCH 045/416] added header file with platformspecific definitions

---
 src/platforms/px4_defines.h | 56 +++++++++++++++++++++++++++++++++++++
 1 file changed, 56 insertions(+)
 create mode 100644 src/platforms/px4_defines.h

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
new file mode 100644
index 0000000000..8d8fd5f3ce
--- /dev/null
+++ b/src/platforms/px4_defines.h
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_defines.h
+ *
+ * Generally used magic defines
+ */
+
+#pragma once
+
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/*
+ * Building for running within the ROS environment
+ */
+#define __EXPORT
+//#define PX4_MAIN_FUNCTION(_prefix)
+#else
+#include <nuttx/config.h>
+//#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+#define ACCEL_DEVICE_PATH	"/dev/accel"
+
+#endif

From 3bfc4a5a52a7777088f21674dd8108026f5f057b Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 13:10:36 +0100
Subject: [PATCH 046/416] removed platform specific includes

---
 src/lib/mathlib/math/Limits.hpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index 713cb51b56..79e127fddd 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,10 +39,7 @@
 
 #pragma once
 
-#ifdef CONFIG_ARCH_ARM
-#include <nuttx/config.h>
-#endif
-
+#include <platforms/px4_config.h>
 #include <stdint.h>
 
 namespace math {

From c8ad06ff9966884242a1ce80eddb4a45efc30c50 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 13:26:24 +0100
Subject: [PATCH 047/416] removed platform specificness

---
 src/modules/systemlib/err.h | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index 2a201ee80b..ca13d62655 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -67,7 +67,6 @@
 
 #include <stdarg.h>
 
-#ifdef CONFIG_ARCH_ARM
 __BEGIN_DECLS
 
 __EXPORT const char *getprogname(void);
@@ -87,8 +86,4 @@ __EXPORT void	vwarnx(const char *fmt, va_list)		__attribute__((format(printf, 1,
 
 __END_DECLS
 
-#else	//we are using ROS (should make a variable!!!)
-#include <ros/ros.h>
-#define warnx ROS_WARN
-#endif
 #endif

From ed409fd53757a43b49758d3ed5573e809a23b113 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 14:55:33 +0100
Subject: [PATCH 048/416] use px4_defines header to distinguish platform

---
 src/lib/geo_lookup/geo_mag_declination.h            | 4 ----
 src/lib/mathlib/math/Limits.hpp                     | 2 +-
 src/lib/mathlib/math/Vector.hpp                     | 2 +-
 src/modules/uORB/topics/actuator_armed.h            | 8 +++-----
 src/modules/uORB/topics/actuator_controls.h         | 6 +-----
 src/modules/uORB/topics/airspeed.h                  | 6 +-----
 src/modules/uORB/topics/manual_control_setpoint.h   | 8 +++-----
 src/modules/uORB/topics/parameter_update.h          | 8 ++------
 src/modules/uORB/topics/vehicle_attitude.h          | 6 +-----
 src/modules/uORB/topics/vehicle_attitude_setpoint.h | 6 +-----
 src/modules/uORB/topics/vehicle_control_mode.h      | 6 +-----
 src/modules/uORB/topics/vehicle_global_position.h   | 7 ++-----
 src/modules/uORB/topics/vehicle_rates_setpoint.h    | 6 +-----
 src/modules/uORB/topics/vehicle_status.h            | 6 +-----
 src/platforms/px4_defines.h                         | 4 +++-
 15 files changed, 22 insertions(+), 63 deletions(-)

diff --git a/src/lib/geo_lookup/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h
index d79b784121..0ac062d6d4 100644
--- a/src/lib/geo_lookup/geo_mag_declination.h
+++ b/src/lib/geo_lookup/geo_mag_declination.h
@@ -40,12 +40,8 @@
 
 #pragma once
 
-#ifdef CONFIG_ARCH_ARM
 __BEGIN_DECLS
 
 __EXPORT float get_mag_declination(float lat, float lon);
 
 __END_DECLS
-#else
-float get_mag_declination(float lat, float lon);
-#endif
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index 79e127fddd..fca4197b8d 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <platforms/px4_config.h>
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 namespace math {
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index b0b03980d3..9accd09070 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -50,7 +50,7 @@
 #include "../CMSIS/Include/arm_math.h"
 #else
 #include <math/eigen_math.h>
-#include <Eigen/Eigen>
+//#include <Eigen/Eigen>
 #endif
 
 namespace math
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 9ec9d10aba..1e10e0ad12 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -42,9 +42,8 @@
 #define TOPIC_ACTUATOR_ARMED_H
 
 #include <stdint.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
+
 /**
  * @addtogroup topics
  * @{
@@ -65,7 +64,6 @@ struct actuator_armed_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(actuator_armed);
-#endif
+
 #endif
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 6c641dbcec..43f7a59ee9 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -47,9 +47,7 @@
 #define TOPIC_ACTUATOR_CONTROLS_H
 
 #include <stdint.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
@@ -72,11 +70,9 @@ struct actuator_controls_s {
  */
 
 /* actuator control sets; this list can be expanded as more controllers emerge */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
-#endif
 
 #endif
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index 4c115a8113..676c37c778 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -40,9 +40,7 @@
 #ifndef TOPIC_AIRSPEED_H_
 #define TOPIC_AIRSPEED_H_
 
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 /**
@@ -65,8 +63,6 @@ struct airspeed_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(airspeed);
-#endif
 
 #endif
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index af5df69792..15b55648d0 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -41,9 +41,8 @@
 #define TOPIC_MANUAL_CONTROL_SETPOINT_H_
 
 #include <stdint.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
+
 /**
  * Switch position
  */
@@ -107,7 +106,6 @@ struct manual_control_setpoint_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(manual_control_setpoint);
-#endif
+
 #endif
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
index 7afb78d49d..fe9c9070fc 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/modules/uORB/topics/parameter_update.h
@@ -40,9 +40,7 @@
 #define TOPIC_PARAMETER_UPDATE_H
 
 #include <stdint.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
@@ -58,8 +56,6 @@ struct parameter_update_s {
  * @}
  */
 
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(parameter_update);
-#endif
 
-#endif
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index 7780988c84..1df1433ac6 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -44,9 +44,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
@@ -89,8 +87,6 @@ struct vehicle_attitude_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_attitude);
-#endif
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 8b5a761433..a503aa0c69 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -42,9 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
@@ -84,8 +82,6 @@ struct vehicle_attitude_setpoint_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_attitude_setpoint);
-#endif
 
 #endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 78de55b7d9..2dd8550bc4 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -48,9 +48,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 #include "vehicle_status.h"
 
 /**
@@ -92,8 +90,6 @@ struct vehicle_control_mode_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_control_mode);
-#endif
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index e8f010924d..f7a4324950 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -45,9 +45,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
@@ -83,7 +81,6 @@ struct vehicle_global_position_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_global_position);
-#endif
+
 #endif
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
index cbfab89d60..e5cecf02b4 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h
@@ -41,9 +41,7 @@
 #define TOPIC_VEHICLE_RATES_SETPOINT_H_
 
 #include <stdint.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics
@@ -64,8 +62,6 @@ struct vehicle_rates_setpoint_s {
 */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_rates_setpoint);
-#endif
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 6bd156ccd6..8e85b48353 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -53,9 +53,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#ifdef CONFIG_ARCH_ARM
-#include "../uORB.h"
-#endif
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics @{
@@ -252,8 +250,6 @@ struct vehicle_status_s {
  */
 
 /* register this as object request broker structure */
-#ifdef CONFIG_ARCH_ARM
 ORB_DECLARE(vehicle_status);
-#endif
 
 #endif
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 8d8fd5f3ce..327d0bea1b 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -36,7 +36,6 @@
  *
  * Generally used magic defines
  */
-
 #pragma once
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
@@ -45,6 +44,7 @@
  */
 #define __EXPORT
 //#define PX4_MAIN_FUNCTION(_prefix)
+#define ORB_DECLARE(x)
 #else
 #include <nuttx/config.h>
 //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }
@@ -53,4 +53,6 @@
 #include "drv_orb_dev.h"
 #define ACCEL_DEVICE_PATH	"/dev/accel"
 
+#include <modules/uORB/uORB.h>
+
 #endif

From a0e2e4e8b30ef64728ff57e56a28b95527f4a1b0 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:01:47 +0100
Subject: [PATCH 049/416] pixhawk specific files

---
 src/modules/fw_att_control/fw_att_control_base.cpp | 1 -
 src/modules/fw_att_control/fw_att_control_base.h   | 2 --
 src/platforms/px4_defines.h                        | 5 -----
 3 files changed, 8 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
index d8ba159693..99780bc7eb 100644
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -56,7 +56,6 @@ FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
 		_setpoint_valid(false), _debug(false) {
 	/* safely initialize structs */
 	_att = {};
-	_accel = {};
 	_att_sp = {};
 	_manual = {};
 	_airspeed = {};
diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h
index 6b2efc46b2..1726c2e3e5 100644
--- a/src/modules/fw_att_control/fw_att_control_base.h
+++ b/src/modules/fw_att_control/fw_att_control_base.h
@@ -54,7 +54,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
 
-#include <drivers/drv_accel.h>
 #include <systemlib/perf_counter.h>
 
 class FixedwingAttitudeControlBase
@@ -78,7 +77,6 @@ protected:
 	int		_control_task;			/**< task handle for sensor task */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct airspeed_s				_airspeed;		/**< airspeed */
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 327d0bea1b..1ff46b97cb 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -48,11 +48,6 @@
 #else
 #include <nuttx/config.h>
 //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }
-
-#include "drv_sensor.h"
-#include "drv_orb_dev.h"
-#define ACCEL_DEVICE_PATH	"/dev/accel"
-
 #include <modules/uORB/uORB.h>
 
 #endif

From 02175f4a678575430a6455e2e5d7f44f6cf64665 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:32:20 +0100
Subject: [PATCH 050/416] removed unnecessary diff content

---
 src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 -
 src/lib/ecl/attitude_fw/ecl_yaw_controller.h  | 1 -
 src/lib/ecl/ecl.h                             | 2 +-
 src/lib/geo/geo.h                             | 8 ++++----
 4 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index dbcabd8478..0799dbe03b 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -53,7 +53,6 @@
 #include <stdint.h>
 #include <systemlib/perf_counter.h>
 
-
 class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index c9e80930f3..a360c14b89 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -52,7 +52,6 @@
 #include <stdint.h>
 #include <systemlib/perf_counter.h>
 
-
 class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index 662e3a39f5..aa3c5000a4 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,6 +38,6 @@
  */
 
 #include <drivers/drv_hrt.h>
+
 #define ecl_absolute_time hrt_absolute_time
 #define ecl_elapsed_time hrt_elapsed_time
-
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index ff2d92389f..fd754a1ae6 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -45,12 +45,12 @@
 
 #pragma once
 
-#ifdef CONFIG_ARCH_ARM
+
 #include "uORB/topics/fence.h"
 #include "uORB/topics/vehicle_global_position.h"
 
 __BEGIN_DECLS
-#endif
+
 #include "geo_lookup/geo_mag_declination.h"
 
 #include <stdbool.h>
@@ -278,6 +278,6 @@ __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
 
-#ifdef CONFIG_ARCH_ARM
+
 __END_DECLS
-#endif
+

From 8e205e0fef4f7a2d8fc832587ae51b685537991d Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:33:02 +0100
Subject: [PATCH 051/416] use px4_config header for multiple platform support

---
 src/modules/uORB/topics/fence.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h
index 6f16c51cf8..a61f078ba1 100644
--- a/src/modules/uORB/topics/fence.h
+++ b/src/modules/uORB/topics/fence.h
@@ -42,7 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

From 05fadc347e4137ecd409b1a6e79fe961f6070ae5 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:33:16 +0100
Subject: [PATCH 052/416] updated

---
 src/platforms/px4_defines.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 1ff46b97cb..535cf67fe9 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -45,6 +45,7 @@
 #define __EXPORT
 //#define PX4_MAIN_FUNCTION(_prefix)
 #define ORB_DECLARE(x)
+
 #else
 #include <nuttx/config.h>
 //#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }

From 19bc137cf319a0380f2cab9224d3cb67b39995e8 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:34:31 +0100
Subject: [PATCH 053/416] removed unnecessary diff content

---
 src/lib/geo/geo.h | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index fd754a1ae6..2311e0a7c9 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -45,7 +45,6 @@
 
 #pragma once
 
-
 #include "uORB/topics/fence.h"
 #include "uORB/topics/vehicle_global_position.h"
 
@@ -54,7 +53,6 @@ __BEGIN_DECLS
 #include "geo_lookup/geo_mag_declination.h"
 
 #include <stdbool.h>
-#include <stdint.h>
 
 #define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
 #define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
@@ -278,6 +276,4 @@ __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
 
-
 __END_DECLS
-

From 284787e02c7471c1f8d651dced0497e964f33454 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:41:07 +0100
Subject: [PATCH 054/416] removed platform specific included, they are not
 needed

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 8 +-------
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 6 ------
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 6 ------
 3 files changed, 1 insertion(+), 19 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index d1f79f0ead..926a8db2ac 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -47,12 +47,6 @@
 #include <mathlib/mathlib.h>
 #include <systemlib/err.h>
 
-#ifdef CONFIG_ARCH_ARM
-#else
-#include <cmath>
-using namespace std;
-#endif
-
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
 	_tc(0.1f),
@@ -81,7 +75,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
-		//perf_count(_nonfinite_input_perf);
+		perf_count(_nonfinite_input_perf);
 		warnx("not controlling pitch");
 		return _rate_setpoint;
 	}
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 30176f92fb..94bd26f03d 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -47,12 +47,6 @@
 #include <mathlib/mathlib.h>
 #include <systemlib/err.h>
 
-#ifdef CONFIG_ARCH_ARM
-#else
-#include <cmath>
-using namespace std;
-#endif
-
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
 	_tc(0.1f),
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 1b4d8486c5..fe03b80651 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -46,12 +46,6 @@
 #include <mathlib/mathlib.h>
 #include <systemlib/err.h>
 
-#ifdef CONFIG_ARCH_ARM
-#else
-#include <cmath>
-using namespace std;
-#endif
-
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
 	_k_p(0.0f),

From 076c1e6238127b7ae14ea9119c79bc286394be8f Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 15:43:47 +0100
Subject: [PATCH 055/416] removed unused include

---
 src/lib/mathlib/math/Quaternion.hpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index d8acc44434..b3cca30c6c 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -45,10 +45,6 @@
 
 #include <math.h>
 
-#ifdef CONFIG_ARCH_ARM
-#include "../CMSIS/Include/arm_math.h"
-#endif
-
 #include "Vector.hpp"
 #include "Matrix.hpp"
 

From 647ec65bd0a164b4bdb38cfc5073e01695676263 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 11 Nov 2014 16:01:01 +0100
Subject: [PATCH 056/416] added ros specific source file for
 performance_counter

---
 src/platforms/ros/perf_counter.cpp | 142 +++++++++++++++++++++++++++++
 1 file changed, 142 insertions(+)
 create mode 100755 src/platforms/ros/perf_counter.cpp

diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp
new file mode 100755
index 0000000000..aa8d85c60c
--- /dev/null
+++ b/src/platforms/ros/perf_counter.cpp
@@ -0,0 +1,142 @@
+/*
+ * perf_counter.c
+
+ *
+ *  Created on: Sep 24, 2014
+ *      Author: roman
+ */
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/perf_counter.h>
+
+
+
+perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name)
+{
+	return NULL;
+}
+
+/**
+ * Free a counter.
+ *
+ * @param handle		The performance counter's handle.
+ */
+void perf_free(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_count(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_begin(perf_counter_t handle)
+{
+
+}
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_end(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_cancel(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Reset a performance counter.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+void		perf_reset(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to stdout
+ *
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+void		perf_print_counter_fd(int fd, perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+void		perf_print_all(int fd)
+{
+
+}
+
+/**
+ * Reset all of the performance counters.
+ */
+void		perf_reset_all(void)
+{
+
+}
+
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+uint64_t	perf_event_count(perf_counter_t handle)
+{
+
+}
+
+

From 44127363b11a9280de818ff28047d170c487af1f Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Wed, 12 Nov 2014 16:01:36 +0100
Subject: [PATCH 057/416] added setter and getter functions for use with
 euroc-gazebo simulator

---
 .../fw_att_control/fw_att_control_base.cpp    | 57 +++++++++++++++++++
 .../fw_att_control/fw_att_control_base.h      |  6 ++
 2 files changed, 63 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
index 99780bc7eb..46fef3e67b 100644
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -267,3 +267,60 @@ void FixedwingAttitudeControlBase::control_attitude() {
 	}
 
 }
+
+void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
+	// watch out, still need to see where we modify attitude for the tailsitter case
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_att.q[0] = quat(0);
+	_att.q[1] = quat(1);
+	_att.q[2] = quat(2);
+	_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_att.R[0][0] = Rot(0,0);
+	_att.R[1][0] = Rot(1,0);
+	_att.R[2][0] = Rot(2,0);
+	_att.R[0][1] = Rot(0,1);
+	_att.R[1][1] = Rot(1,1);
+	_att.R[2][1] = Rot(2,1);
+	_att.R[0][2] = Rot(0,2);
+	_att.R[1][2] = Rot(1,2);
+	_att.R[2][2] = Rot(2,2);
+
+	_att.R_valid = true;
+}
+void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	_att.rollspeed  = angular_rate(0);
+	_att.pitchspeed = angular_rate(1);
+	_att.yawspeed   = angular_rate(2);
+}
+void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
+	_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_att_sp.R_body[2][2] = Rot_sp(2,2);
+}
+void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}
diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h
index 1726c2e3e5..bde1b755f5 100644
--- a/src/modules/fw_att_control/fw_att_control_base.h
+++ b/src/modules/fw_att_control/fw_att_control_base.h
@@ -140,6 +140,12 @@ protected:
 
 	void control_attitude();
 
+	// setters and getters for interface with euroc-gazebo simulator
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	
 };
 
 #endif /* FW_ATT_CONTROL_BASE_H_ */

From 794a89e711a90a62510f6bdde8ee59511237f6ca Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 13 Nov 2014 10:51:54 +0100
Subject: [PATCH 058/416] added more digits to pi/2

---
 src/lib/mathlib/math/Matrix.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 832be66dbf..9fc3c78c8a 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -51,7 +51,7 @@
 #else
 #include <math/eigen_math.h>
 #include <Eigen/Eigen>
-#define M_PI_2_F 1.570769
+#define M_PI_2_F 1.5707963267948966192
 #endif
 
 namespace math {

From 003b326c7731b0f3b9b139ae008d6e4d07cefab8 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 13 Nov 2014 11:16:22 +0100
Subject: [PATCH 059/416] applied fix_code_style.sh

---
 src/lib/mathlib/math/Matrix.hpp | 113 ++++++++++++++++++--------------
 1 file changed, 64 insertions(+), 49 deletions(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 9fc3c78c8a..806f5933ad 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -51,10 +51,11 @@
 #else
 #include <math/eigen_math.h>
 #include <Eigen/Eigen>
-#define M_PI_2_F 1.5707963267948966192
+#define M_PI_2_F 1.5707963267948966192f
 #endif
 
-namespace math {
+namespace math
+{
 
 template<unsigned int M, unsigned int N>
 class __EXPORT Matrix;
@@ -83,9 +84,8 @@ public:
 	 * Initializes the elements to zero.
 	 */
 	MatrixBase() :
-	data {},
-	arm_mat {M, N, &data[0][0]}
-	{
+		data {},
+	arm_mat {M, N, &data[0][0]} {
 	}
 
 	virtual ~MatrixBase() {};
@@ -94,20 +94,17 @@ public:
 	 * copyt ctor
 	 */
 	MatrixBase(const MatrixBase<M, N> &m) :
-	arm_mat {M, N, &data[0][0]}
-	{
+		arm_mat {M, N, &data[0][0]} {
 		memcpy(data, m.data, sizeof(data));
 	}
 
 	MatrixBase(const float *d) :
-	arm_mat {M, N, &data[0][0]}
-	{
+		arm_mat {M, N, &data[0][0]} {
 		memcpy(data, d, sizeof(data));
 	}
 
 	MatrixBase(const float d[M][N]) :
-	arm_mat {M, N, &data[0][0]}
-	{
+		arm_mat {M, N, &data[0][0]} {
 		memcpy(data, d, sizeof(data));
 	}
 
@@ -158,9 +155,10 @@ public:
 	 */
 	bool operator ==(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		if (data[i][j] != m.data[i][j])
-		return false;
+			for (unsigned int j = 0; j < N; j++)
+				if (data[i][j] != m.data[i][j]) {
+					return false;
+				}
 
 		return true;
 	}
@@ -170,9 +168,10 @@ public:
 	 */
 	bool operator !=(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		if (data[i][j] != m.data[i][j])
-		return true;
+			for (unsigned int j = 0; j < N; j++)
+				if (data[i][j] != m.data[i][j]) {
+					return true;
+				}
 
 		return false;
 	}
@@ -192,8 +191,9 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-		for (unsigned int j = 0; j < M; j++)
-		res.data[i][j] = -data[i][j];
+			for (unsigned int j = 0; j < M; j++) {
+				res.data[i][j] = -data[i][j];
+			}
 
 		return res;
 	}
@@ -205,16 +205,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-		for (unsigned int j = 0; j < M; j++)
-		res.data[i][j] = data[i][j] + m.data[i][j];
+			for (unsigned int j = 0; j < M; j++) {
+				res.data[i][j] = data[i][j] + m.data[i][j];
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-		for (unsigned int j = 0; j < M; j++)
-		data[i][j] += m.data[i][j];
+			for (unsigned int j = 0; j < M; j++) {
+				data[i][j] += m.data[i][j];
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -226,16 +228,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		res.data[i][j] = data[i][j] - m.data[i][j];
+			for (unsigned int j = 0; j < N; j++) {
+				res.data[i][j] = data[i][j] - m.data[i][j];
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-		for (unsigned int j = 0; j < M; j++)
-		data[i][j] -= m.data[i][j];
+			for (unsigned int j = 0; j < M; j++) {
+				data[i][j] -= m.data[i][j];
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -247,8 +251,9 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		res.data[i][j] = data[i][j] * num;
+			for (unsigned int j = 0; j < N; j++) {
+				res.data[i][j] = data[i][j] * num;
+			}
 
 		return res;
 
@@ -256,8 +261,9 @@ public:
 
 	Matrix<M, N> &operator *=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		data[i][j] *= num;
+			for (unsigned int j = 0; j < N; j++) {
+				data[i][j] *= num;
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -266,16 +272,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		res[i][j] = data[i][j] / num;
+			for (unsigned int j = 0; j < N; j++) {
+				res[i][j] = data[i][j] / num;
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator /=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-		for (unsigned int j = 0; j < N; j++)
-		data[i][j] /= num;
+			for (unsigned int j = 0; j < N; j++) {
+				data[i][j] /= num;
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -290,9 +298,11 @@ public:
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
 #else
-		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
-		Eigen::Matrix<float,N,P,Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float,N,P,Eigen::RowMajor> >(m.arm_mat.pData);
-		Eigen::Matrix<float,M,P,Eigen::RowMajor> Product = Me * Him;
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
+				(m.arm_mat.pData);
+		Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
 		Matrix<M, P> res(Product.data());
 		return res;
 #endif
@@ -307,7 +317,8 @@ public:
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
 #else
-		Eigen::Matrix<float,N,M,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,N,M,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
+				(this->arm_mat.pData);
 		Me.transposeInPlace();
 		Matrix<N, M> res(Me.data());
 		return res;
@@ -323,8 +334,9 @@ public:
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
 #else
-		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
-		Eigen::Matrix<float,M,N,Eigen::RowMajor> MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
 		Matrix<M, N> res(MyInverse.data());
 		return res;
 #endif
@@ -344,16 +356,18 @@ public:
 		memset(data, 0, sizeof(data));
 		unsigned int n = (M < N) ? M : N;
 
-		for (unsigned int i = 0; i < n; i++)
-		data[i][i] = 1;
+		for (unsigned int i = 0; i < n; i++) {
+			data[i][i] = 1;
+		}
 	}
 
 	void print(void) {
 		for (unsigned int i = 0; i < M; i++) {
 			printf("[ ");
 
-			for (unsigned int j = 0; j < N; j++)
-			printf("%.3f\t", data[i][j]);
+			for (unsigned int j = 0; j < N; j++) {
+				printf("%.3f\t", data[i][j]);
+			}
 
 			printf(" ]\n");
 		}
@@ -391,8 +405,9 @@ public:
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
 #else
 		//probably nicer if this could go into a function like "eigen_mat_mult" or so
-		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
-		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
 		Eigen::VectorXf Product = Me * Vec;
 		Vector<M> res(Product.data());
 #endif
@@ -427,8 +442,8 @@ public:
 	 */
 	Vector<3> operator *(const Vector<3> &v) const {
 		Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
-				data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
-				data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
+			      data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
+			      data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
 		return res;
 	}
 

From d9314653826d36b0a9723d69a291220245b3f699 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 13 Nov 2014 13:48:18 +0100
Subject: [PATCH 060/416] added ifdef guard

---
 src/lib/mathlib/math/Limits.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index c593936ce6..e16f33bd68 100644
--- a/src/lib/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -45,7 +45,10 @@
 
 
 namespace math {
-#define M_PI_F 3.14159265358979323846
+
+#ifndef CONFIG_ARCH_ARM	
+#define M_PI_F 3.14159265358979323846f
+#endif
 
 float __EXPORT min(float val1, float val2)
 {

From efe0938e9990225636e5fed78e945ded5f24220f Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Thu, 13 Nov 2014 13:48:40 +0100
Subject: [PATCH 061/416] removed comment

---
 src/lib/mathlib/math/Vector.hpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 9accd09070..57b45e3ab2 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -50,7 +50,6 @@
 #include "../CMSIS/Include/arm_math.h"
 #else
 #include <math/eigen_math.h>
-//#include <Eigen/Eigen>
 #endif
 
 namespace math

From 1e36de61579dd35fe46a069520b7c8970c3cb7cd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 24 Nov 2014 15:57:48 +0100
Subject: [PATCH 062/416] ignore build folder

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index 8b09e4783f..69112ee1fa 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,6 +17,7 @@
 .~lock.*
 Archives/*
 Build/*
+build/*
 core
 cscope.out
 Firmware.sublime-workspace

From 3f36d30a3413cd70096e953a2c9ea0ded65bf24e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 24 Nov 2014 15:58:06 +0100
Subject: [PATCH 063/416] wrapped subscriber

---
 CMakeLists.txt                         |  1 +
 src/examples/subscriber/subscriber.cpp |  4 +--
 src/include/px4.h                      |  2 ++
 src/platforms/nuttx/px4_nodehandle.cpp | 39 +++++++++++++++++++++++
 src/platforms/px4_nodehandle.h         | 18 ++++++++++-
 src/platforms/px4_publisher.h          |  1 +
 src/platforms/px4_subscriber.h         | 16 +++++++++-
 src/platforms/ros/px4_nodehandle.cpp   | 44 ++++++++++++++++++++++++++
 src/platforms/ros/px4_publisher.cpp    |  1 +
 src/platforms/ros/px4_subscriber.cpp   |  1 +
 10 files changed, 123 insertions(+), 4 deletions(-)
 create mode 100644 src/platforms/nuttx/px4_nodehandle.cpp
 create mode 100644 src/platforms/ros/px4_nodehandle.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 36c2ffeff8..8eb81c92ca 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -104,6 +104,7 @@ add_library(px4
   src/platforms/ros/px4_ros_impl.cpp
   src/platforms/ros/px4_publisher.cpp
   src/platforms/ros/px4_subscriber.cpp
+  src/platforms/ros/px4_nodehandle.cpp
 )
 
 target_link_libraries(px4
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index bf16bf84e1..39059daa30 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -57,7 +57,7 @@ int main(int argc, char **argv)
 	 * The first NodeHandle constructed will fully initialize this node, and the last
 	 * NodeHandle destructed will close down the node.
 	 */
-	ros::NodeHandle n;
+	px4::NodeHandle n;
 
 	/**
 	 * The subscribe() call is how you tell ROS that you want to receive messages
@@ -74,7 +74,7 @@ int main(int argc, char **argv)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
+	px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback);
 
 	/**
 	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
diff --git a/src/include/px4.h b/src/include/px4.h
index 0aba2ee77d..bb97f2a8a9 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -58,3 +58,5 @@
 
 #include "../platforms/px4_defines.h"
 #include "../platforms/px4_middleware.h"
+#include "../platforms/px4_nodehandle.h"
+#include "../platforms/px4_subscriber.h"
diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
new file mode 100644
index 0000000000..9d43daa498
--- /dev/null
+++ b/src/platforms/nuttx/px4_nodehandle.cpp
@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Nodehandle
+ */
+#include <px4_nodehandle.h>
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index d278828b7c..df198615c5 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -36,11 +36,27 @@
  *
  * PX4 Middleware Wrapper Node Handle
  */
+#pragma once
+#include <px4_subscriber.h>
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#include "ros/ros.h"
+#endif
 
 namespace px4
 {
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+class NodeHandle : private ros::NodeHandle
+{
+public:
+	template<class M>
+	Subscriber* subscribe(const char *topic, void(*fp)(M)) {
+		ros::NodeHandle::subscribe("rc_channels", 1000, fp);
+		return new Subscriber();
+	}
+};
+#else
 class NodeHandle
 {
-
 };
+#endif
 }
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 1b09521554..78d2a744b7 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -36,6 +36,7 @@
  *
  * PX4 Middleware Wrapper Node Handle
  */
+#pragma once
 
 namespace px4
 {
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 8759f8b05d..8f883154ee 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -36,11 +36,25 @@
  *
  * PX4 Middleware Wrapper Subscriber
  */
+#pragma once
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#include "ros/ros.h"
+#endif
 
 namespace px4
 {
+
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class Subscriber
 {
-
+public:
+	Subscriber() {};
+	~Subscriber() {};
 };
+#else
+class Subscriber
+{
+};
+#endif
+
 }
diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp
new file mode 100644
index 0000000000..6ac3c76d31
--- /dev/null
+++ b/src/platforms/ros/px4_nodehandle.cpp
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Nodehandle
+ */
+#include <px4_nodehandle.h>
+
+namespace px4
+{
+}
+
diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp
index ab6035b225..f02dbe4c9a 100644
--- a/src/platforms/ros/px4_publisher.cpp
+++ b/src/platforms/ros/px4_publisher.cpp
@@ -36,5 +36,6 @@
  *
  * PX4 Middleware Wrapper for Publisher
  */
+#include <px4_publisher.h>
 
 
diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp
index 088c08fdb6..d040b860d2 100644
--- a/src/platforms/ros/px4_subscriber.cpp
+++ b/src/platforms/ros/px4_subscriber.cpp
@@ -37,4 +37,5 @@
  * PX4 Middleware Wrapper Subscriber
  */
 
+#include <px4_subscriber.h>
 

From 1826fa5d39a5feb2a7c044102c899c23c1f12453 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 24 Nov 2014 17:38:27 +0100
Subject: [PATCH 064/416] improve subsciber api

---
 src/examples/subscriber/subscriber.cpp | 1 +
 src/platforms/px4_nodehandle.h         | 5 +++--
 src/platforms/px4_subscriber.h         | 5 ++++-
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 39059daa30..5d1d14d7fa 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -75,6 +75,7 @@ int main(int argc, char **argv)
 	 * away the oldest ones.
 	 */
 	px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback);
+	PX4_INFO("subscribed");
 
 	/**
 	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index df198615c5..809d0eb150 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -50,8 +50,9 @@ class NodeHandle : private ros::NodeHandle
 public:
 	template<class M>
 	Subscriber* subscribe(const char *topic, void(*fp)(M)) {
-		ros::NodeHandle::subscribe("rc_channels", 1000, fp);
-		return new Subscriber();
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp);
+		//XXX create list here, for ros and nuttx
+		return new Subscriber(ros_sub);
 	}
 };
 #else
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 8f883154ee..8933b2ab22 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -47,8 +47,11 @@ namespace px4
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class Subscriber
 {
+private:
+	ros::Subscriber _ros_sub;
 public:
-	Subscriber() {};
+	Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub)
+	{}
 	~Subscriber() {};
 };
 #else

From e2f846ee2fb20193ec73e1ea6ebbbd1fa967166e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 24 Nov 2014 18:53:08 +0100
Subject: [PATCH 065/416] create list of subscribers

---
 src/examples/subscriber/subscriber.cpp |  2 +-
 src/include/containers/List.hpp        |  1 +
 src/platforms/px4_nodehandle.h         | 14 ++++++++++++--
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 5d1d14d7fa..828b1f9379 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -74,7 +74,7 @@ int main(int argc, char **argv)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback);
+	px4::Subscriber sub = n.subscribe("rc_channels", rc_channels_callback);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp
index 13cbda9382..5c0ba59fd3 100644
--- a/src/include/containers/List.hpp
+++ b/src/include/containers/List.hpp
@@ -38,6 +38,7 @@
  */
 
 #pragma once
+#include <cstddef>
 
 template<class T>
 class __EXPORT ListNode
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 809d0eb150..9bbf5c7240 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -40,6 +40,7 @@
 #include <px4_subscriber.h>
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 #include "ros/ros.h"
+#include <list>
 #endif
 
 namespace px4
@@ -48,12 +49,21 @@ namespace px4
 class NodeHandle : private ros::NodeHandle
 {
 public:
+	NodeHandle () :
+		ros::NodeHandle(),
+		_subs()
+	{}
+
 	template<class M>
-	Subscriber* subscribe(const char *topic, void(*fp)(M)) {
+	Subscriber subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp);
 		//XXX create list here, for ros and nuttx
-		return new Subscriber(ros_sub);
+		Subscriber sub(ros_sub);
+		_subs.push_back(sub);
+		return sub;
 	}
+private:
+	std::list<Subscriber> _subs;
 };
 #else
 class NodeHandle

From 0a3492fc328280422df9472d3d8a586d92242feb Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 25 Nov 2014 09:20:57 +0100
Subject: [PATCH 066/416] define default queue size

---
 src/platforms/px4_nodehandle.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 9bbf5c7240..6880d74f65 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -41,6 +41,7 @@
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 #include "ros/ros.h"
 #include <list>
+#define QUEUE_SIZE_DEFAULT 1000
 #endif
 
 namespace px4
@@ -56,7 +57,7 @@ public:
 
 	template<class M>
 	Subscriber subscribe(const char *topic, void(*fp)(M)) {
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp);
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp);
 		//XXX create list here, for ros and nuttx
 		Subscriber sub(ros_sub);
 		_subs.push_back(sub);

From 978013bbb8d67e295d92a54e16f7728013722e92 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 25 Nov 2014 09:56:18 +0100
Subject: [PATCH 067/416] px4 wrapper for ros publisher

---
 src/examples/publisher/publisher.cpp |  5 +++--
 src/platforms/px4_nodehandle.h       | 16 +++++++++++++---
 src/platforms/px4_publisher.h        | 18 +++++++++++++++++-
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 91e0631622..6869e765bd 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -38,7 +38,7 @@ int main(int argc, char **argv)
 
 	px4::init(argc, argv, "px4_publisher");
 
-	ros::NodeHandle n;
+	px4::NodeHandle n;
 
 	/**
 	 * The advertise() function is how you tell ROS that you want to
@@ -57,7 +57,8 @@ int main(int argc, char **argv)
 	 * than we can send them, the number here specifies how many messages to
 	 * buffer up before throwing some away.
 	 */
-	ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
+	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels");
+
 
 	px4::Rate loop_rate(10);
 
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 6880d74f65..bfda636b0f 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -38,6 +38,7 @@
  */
 #pragma once
 #include <px4_subscriber.h>
+#include <px4_publisher.h>
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 #include "ros/ros.h"
 #include <list>
@@ -52,19 +53,28 @@ class NodeHandle : private ros::NodeHandle
 public:
 	NodeHandle () :
 		ros::NodeHandle(),
-		_subs()
+		_subs(),
+		_pubs()
 	{}
 
-	template<class M>
+	template<typename M>
 	Subscriber subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp);
-		//XXX create list here, for ros and nuttx
 		Subscriber sub(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
+
+	template<typename M>
+	Publisher advertise(const char *topic) {
+		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, QUEUE_SIZE_DEFAULT);
+		Publisher pub(ros_pub);
+		_pubs.push_back(pub);
+		return pub;
+	}
 private:
 	std::list<Subscriber> _subs;
+	std::list<Publisher> _pubs;
 };
 #else
 class NodeHandle
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 78d2a744b7..7995251902 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -37,11 +37,27 @@
  * PX4 Middleware Wrapper Node Handle
  */
 #pragma once
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#include "ros/ros.h"
+#endif
 
 namespace px4
 {
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class Publisher
 {
-
+private:
+	ros::Publisher _ros_pub;
+public:
+	Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
+	{}
+	~Publisher() {};
+	template<typename M>
+	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
 };
+#else
+class Publisher
+{
+};
+#endif
 }

From c167df90380fdd99d1b56024c4de104a3f0a2f85 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 25 Nov 2014 10:19:18 +0100
Subject: [PATCH 068/416] ros wrapper: small reordering

---
 src/platforms/px4_nodehandle.h | 14 +++++++++++---
 src/platforms/px4_publisher.h  |  4 ++--
 src/platforms/px4_subscriber.h |  4 ++--
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index bfda636b0f..d668844453 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -37,12 +37,19 @@
  * PX4 Middleware Wrapper Node Handle
  */
 #pragma once
+
+/* includes for all platforms */
 #include <px4_subscriber.h>
 #include <px4_publisher.h>
+
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/* includes when building for ros */
 #include "ros/ros.h"
 #include <list>
-#define QUEUE_SIZE_DEFAULT 1000
+#include <inttypes.h>
+#else
+/* includes when building for NuttX */
+
 #endif
 
 namespace px4
@@ -59,7 +66,7 @@ public:
 
 	template<typename M>
 	Subscriber subscribe(const char *topic, void(*fp)(M)) {
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp);
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
 		Subscriber sub(ros_sub);
 		_subs.push_back(sub);
 		return sub;
@@ -67,12 +74,13 @@ public:
 
 	template<typename M>
 	Publisher advertise(const char *topic) {
-		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, QUEUE_SIZE_DEFAULT);
+		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
 		Publisher pub(ros_pub);
 		_pubs.push_back(pub);
 		return pub;
 	}
 private:
+	static const uint32_t kQueueSizeDefault = 1000;
 	std::list<Subscriber> _subs;
 	std::list<Publisher> _pubs;
 };
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 7995251902..72f69a5af3 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -46,14 +46,14 @@ namespace px4
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class Publisher
 {
-private:
-	ros::Publisher _ros_pub;
 public:
 	Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
 	{}
 	~Publisher() {};
 	template<typename M>
 	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+private:
+	ros::Publisher _ros_pub;
 };
 #else
 class Publisher
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 8933b2ab22..78be3ebff6 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -47,12 +47,12 @@ namespace px4
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class Subscriber
 {
-private:
-	ros::Subscriber _ros_sub;
 public:
 	Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub)
 	{}
 	~Subscriber() {};
+private:
+	ros::Subscriber _ros_sub;
 };
 #else
 class Subscriber

From 55cf2fc61c7b90725cd960f9c7d72737024f1cfc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 25 Nov 2014 11:50:35 +0100
Subject: [PATCH 069/416] WIP, towards more px4 compatibility, first macros

---
 src/examples/publisher/publisher.cpp   |  2 +-
 src/examples/subscriber/subscriber.cpp |  2 +-
 src/include/px4.h                      |  5 ++++
 src/modules/uORB/Publication.cpp       |  1 +
 src/modules/uORB/Subscription.cpp      |  1 +
 src/platforms/px4_nodehandle.h         | 35 +++++++++++++++++++++++---
 src/platforms/px4_publisher.h          | 14 ++++++++++-
 src/platforms/px4_subscriber.h         | 17 +++++++++++--
 8 files changed, 69 insertions(+), 8 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 6869e765bd..c09cca1fca 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -57,7 +57,7 @@ int main(int argc, char **argv)
 	 * than we can send them, the number here specifies how many messages to
 	 * buffer up before throwing some away.
 	 */
-	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels");
+	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>(PX4_TOPIC(rc_channels));
 
 
 	px4::Rate loop_rate(10);
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 828b1f9379..b91859027a 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -74,7 +74,7 @@ int main(int argc, char **argv)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	px4::Subscriber sub = n.subscribe("rc_channels", rc_channels_callback);
+	px4::Subscriber sub = n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/include/px4.h b/src/include/px4.h
index bb97f2a8a9..391972b128 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -46,14 +46,19 @@
  * Building for running within the ROS environment
  */
 #include "ros/ros.h"
+
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
+#define PX4_TOPIC(name) #name
 #else
 /*
  * Building for NuttX
  */
+#include <uORB/uORB.h>
+
 #define PX4_WARN warnx
 #define PX4_INFO warnx
+#define PX4_TOPIC(name) ORB_ID(name)
 #endif
 
 #include "../platforms/px4_defines.h"
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index cd0b30dd61..302b99c69f 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -78,5 +78,6 @@ template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
+template class __EXPORT Publication<rc_channels_s>;
 
 }
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 44b6debc7e..3406a66d36 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -101,5 +101,6 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
 template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
 template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+template class __EXPORT Subscription<rc_channels_s>;
 
 } // namespace uORB
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index d668844453..fa2c8d6a47 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -49,21 +49,26 @@
 #include <inttypes.h>
 #else
 /* includes when building for NuttX */
-
+#include <containers/List.hpp>
 #endif
 
 namespace px4
 {
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
-class NodeHandle : private ros::NodeHandle
+class NodeHandle :
+	private ros::NodeHandle
 {
 public:
-	NodeHandle () :
+	NodeHandle() :
 		ros::NodeHandle(),
 		_subs(),
 		_pubs()
 	{}
 
+	~NodeHandle() {
+		//XXX empty lists
+	};
+
 	template<typename M>
 	Subscriber subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
@@ -87,6 +92,30 @@ private:
 #else
 class NodeHandle
 {
+public:
+	NodeHandle() :
+		_subs(),
+		_pubs()
+	{}
+
+	~NodeHandle() {};
+
+	template<typename M>
+	Subscriber subscribe(const char *topic, void(*fp)(M)) {
+		Subscriber sub(&_subs, , interval);
+		return sub;
+	}
+
+	template<typename M>
+	Publisher advertise(const char *topic) {
+		Publisher pub(ros_pub);
+		_pubs.push_back(pub);
+		return pub;
+	}
+private:
+	List<Subscriber> _subs;
+	List<Publisher> _pubs;
+
 };
 #endif
 }
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 72f69a5af3..53e63b695d 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -38,7 +38,11 @@
  */
 #pragma once
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/* includes when building for ros */
 #include "ros/ros.h"
+#else
+/* includes when building for NuttX */
+#include <uORB/Publication.hpp>
 #endif
 
 namespace px4
@@ -56,7 +60,15 @@ private:
 	ros::Publisher _ros_pub;
 };
 #else
-class Publisher
+template<typename M>
+class Publisher :
+	public uORB::Publication<M>
+public:
+	Publisher(List<SubscriptionBase *> * list,
+			const struct orb_metadata *meta, unsigned interval) :
+		uORB::Publication(list, meta)
+	{}
+	~Publisher() {};
 {
 };
 #endif
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 78be3ebff6..6312e0cbea 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -38,7 +38,11 @@
  */
 #pragma once
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/* includes when building for ros */
 #include "ros/ros.h"
+#else
+/* includes when building for NuttX */
+#include <uORB/Subscription.hpp>
 #endif
 
 namespace px4
@@ -48,14 +52,23 @@ namespace px4
 class Subscriber
 {
 public:
-	Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub)
+	Subscriber(ros::Subscriber ros_sub) :
+		_ros_sub(ros_sub)
 	{}
 	~Subscriber() {};
 private:
 	ros::Subscriber _ros_sub;
 };
 #else
-class Subscriber
+template<typename M>
+class Subscriber :
+	public uORB::Subscription<M>
+public:
+	Subscriber(List<SubscriptionBase *> * list,
+			const struct orb_metadata *meta, unsigned interval) :
+		uORB::Subsciption(list, meta, interval)
+	{}
+	~Subscriber() {};
 {
 };
 #endif

From 579ec36c28d4cbf08d93531853b2b8fd3f3461ac Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 25 Nov 2014 15:56:05 +0100
Subject: [PATCH 070/416] fix order of arguments (merge fix)

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index bffeefc1ff..ecdab29366 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -52,7 +52,7 @@ mTecs::mTecs() :
 	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
-	_status(&getPublications(), ORB_ID(tecs_status)),
+	_status(ORB_ID(tecs_status), &getPublications()),
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),

From e7c1e5b1ff7b1bbdc11ab2cae6b99fe459487119 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 26 Nov 2014 11:36:23 +0100
Subject: [PATCH 071/416] wip, working on the nuttx wrapper

---
 src/examples/publisher/publisher.cpp   | 13 +++++++------
 src/examples/subscriber/subscriber.cpp | 11 ++++++-----
 src/include/px4.h                      | 10 ++++------
 src/modules/uORB/Publication.cpp       |  1 +
 src/modules/uORB/Publication.hpp       | 10 +++++-----
 src/modules/uORB/Subscription.cpp      |  1 +
 src/platforms/nuttx/module.mk          |  3 ++-
 src/platforms/nuttx/px4_nodehandle.cpp |  2 +-
 src/platforms/nuttx/px4_nuttx_impl.cpp |  8 +++++---
 src/platforms/nuttx/px4_publisher.cpp  |  1 +
 src/platforms/nuttx/px4_subscriber.cpp |  2 +-
 src/platforms/px4_defines.h            | 23 ++++++++++++++++++++---
 src/platforms/px4_middleware.h         | 11 ++++++-----
 src/platforms/px4_nodehandle.h         | 23 ++++++++++++-----------
 src/platforms/px4_publisher.h          | 21 +++++++++++++++------
 src/platforms/px4_subscriber.h         | 20 ++++++++++++++------
 16 files changed, 101 insertions(+), 59 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index c09cca1fca..68fd8a5c3f 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -26,14 +26,15 @@
  */
 
 #include <px4.h>
-#include <px4/rc_channels.h>
 
-#include <sstream>
+using namespace px4;
 
 /**
  * This tutorial demonstrates simple sending of messages over the PX4 middleware system.
  */
-int main(int argc, char **argv)
+// __EXPORT bool task_should_exit;
+
+PX4_MAIN_FUNCTION(publisher)
 {
 
 	px4::init(argc, argv, "px4_publisher");
@@ -57,7 +58,7 @@ int main(int argc, char **argv)
 	 * than we can send them, the number here specifies how many messages to
 	 * buffer up before throwing some away.
 	 */
-	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>(PX4_TOPIC(rc_channels));
+	px4::Publisher * rc_channels_pub = n.advertise<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels));
 
 
 	px4::Rate loop_rate(10);
@@ -72,7 +73,7 @@ int main(int argc, char **argv)
 		/**
 		 * This is a message object. You stuff it with data, and then publish it.
 		 */
-		px4::rc_channels msg;
+		PX4_TOPIC_T(rc_channels) msg;
 
 		msg.timestamp_last_valid = px4::get_time_micros();
 		PX4_INFO("%lu", msg.timestamp_last_valid);
@@ -83,7 +84,7 @@ int main(int argc, char **argv)
 		 * given as a template parameter to the advertise<>() call, as was done
 		 * in the constructor above.
 		 */
-		rc_channels_pub.publish(msg);
+		rc_channels_pub->publish(msg);
 
 		px4::spin_once();
 
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index b91859027a..cdadaf2bcd 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -26,19 +26,20 @@
  */
 
 #include <px4.h>
-#include "px4/rc_channels.h"
+
+using namespace px4;
 
 /**
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  */
-void rc_channels_callback(const px4::rc_channels::ConstPtr &msg)
+void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg)
 {
 	PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid);
 }
 
-PX4_MAIN_FUNCTION(subscriber)
+// __EXPORT bool task_should_exit;
 
-int main(int argc, char **argv)
+PX4_MAIN_FUNCTION(subscriber)
 {
 	/**
 	 * The ros::init() function needs to see argc and argv so that it can perform
@@ -74,7 +75,7 @@ int main(int argc, char **argv)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	px4::Subscriber sub = n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback);
+	n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/include/px4.h b/src/include/px4.h
index 391972b128..22d661b17c 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -46,19 +46,17 @@
  * Building for running within the ROS environment
  */
 #include "ros/ros.h"
+#include "px4/rc_channels.h"
 
-#define PX4_WARN ROS_WARN
-#define PX4_INFO ROS_INFO
-#define PX4_TOPIC(name) #name
 #else
 /*
  * Building for NuttX
  */
+#include <nuttx/config.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
 
-#define PX4_WARN warnx
-#define PX4_INFO warnx
-#define PX4_TOPIC(name) ORB_ID(name)
 #endif
 
 #include "../platforms/px4_defines.h"
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index 12ef83aa07..05605417d3 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -48,6 +48,7 @@
 #include "topics/actuator_outputs.h"
 #include "topics/encoders.h"
 #include "topics/tecs_status.h"
+#include "topics/rc_channels.h"
 
 namespace uORB {
 
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index 1c48929e41..fd1ee4deca 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -59,7 +59,7 @@ public:
 	 * Constructor
 	 *
 	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * @param meta The uORB metadata (usually from the ORB_ID()
 	 * 	macro) for the topic.
 	 */
 	PublicationBase(const struct orb_metadata *meta) :
@@ -96,7 +96,7 @@ protected:
 	orb_advert_t _handle;
 };
 
-/** 
+/**
  * alias class name so it is clear that the base class
  * can be used by itself if desired
  */
@@ -114,9 +114,9 @@ public:
 	 * Constructor
 	 *
 	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * @param meta The uORB metadata (usually from the ORB_ID()
 	 * 	macro) for the topic.
-	 * @param list 	A pointer to a list of subscriptions 
+	 * @param list 	A pointer to a list of subscriptions
 	 * 	that this should be appended to.
 	 */
 	PublicationNode(const struct orb_metadata *meta,
@@ -144,7 +144,7 @@ public:
 	/**
 	 * Constructor
 	 *
-	 * @param meta The uORB metadata (usually from 
+	 * @param meta The uORB metadata (usually from
 	 * 	the ORB_ID() macro) for the topic.
 	 * @param list A list interface for adding to
 	 * 	list during construction
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index a681ccb30a..61609d0092 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -52,6 +52,7 @@
 #include "topics/vehicle_local_position.h"
 #include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_rates_setpoint.h"
+#include "topics/rc_channels.h"
 
 namespace uORB
 {
diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk
index 128f0e7346..1c0ad7aa45 100644
--- a/src/platforms/nuttx/module.mk
+++ b/src/platforms/nuttx/module.mk
@@ -37,6 +37,7 @@
 
 SRCS		 = 	px4_nuttx_impl.cpp \
 			px4_publisher.cpp \
-			px4_subscriber.cpp
+			px4_subscriber.cpp \
+			px4_nodehandle.cpp
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
index 9d43daa498..473a5cf777 100644
--- a/src/platforms/nuttx/px4_nodehandle.cpp
+++ b/src/platforms/nuttx/px4_nodehandle.cpp
@@ -36,4 +36,4 @@
  *
  * PX4 Middleware Wrapper Nodehandle
  */
-#include <px4_nodehandle.h>
+#include <platforms/px4_nodehandle.h>
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 3a65297169..4b87f68fee 100644
--- a/src/platforms/nuttx/px4_nuttx_impl.cpp
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -38,6 +38,7 @@
  */
 
 #include <px4.h>
+#include <drivers/drv_hrt.h>
 
 extern bool task_should_exit;
 
@@ -46,8 +47,7 @@ namespace px4
 
 void init(int argc, char *argv[], const char *process_name)
 {
-	px4_warn("process: %s", process_name);
-	return 0;
+	PX4_WARN("process: %s", process_name);
 }
 
 uint64_t get_time_micros()
@@ -57,7 +57,9 @@ uint64_t get_time_micros()
 
 bool ok()
 {
-	return !task_should_exit;
+	// return !task_should_exit;
+	//XXX
+	return true;
 }
 
 void spin_once()
diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp
index ab6035b225..3bd70272f1 100644
--- a/src/platforms/nuttx/px4_publisher.cpp
+++ b/src/platforms/nuttx/px4_publisher.cpp
@@ -36,5 +36,6 @@
  *
  * PX4 Middleware Wrapper for Publisher
  */
+#include <platforms/px4_publisher.h>
 
 
diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp
index 088c08fdb6..426e646c9f 100644
--- a/src/platforms/nuttx/px4_subscriber.cpp
+++ b/src/platforms/nuttx/px4_subscriber.cpp
@@ -36,5 +36,5 @@
  *
  * PX4 Middleware Wrapper Subscriber
  */
-
+#include <platforms/px4_subscriber.h>
 
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 48234766f0..d4dc714536 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -44,8 +44,25 @@
  * Building for running within the ROS environment
  */
 #define __EXPORT
-#define PX4_MAIN_FUNCTION(_prefix)
+// #define PX4_MAIN_FUNCTION(_prefix)
+#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
+#define PX4_WARN ROS_WARN
+#define PX4_INFO ROS_INFO
+#define PX4_TOPIC(name) #name
+#define PX4_TOPIC_T(name) name
+
 #else
-#include <nuttx/config.h>
-#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }
+/*
+ * Building for NuttX
+ */
+
+// #define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##_main(int argc, char **argv)() { return main(argc, argv); }
+// #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) { return main(argc, argv); }
+#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
+#define PX4_WARN warnx
+#define PX4_WARN warnx
+#define PX4_INFO warnx
+#define PX4_TOPIC(name) ORB_ID(name)
+#define PX4_TOPIC_T(name) name##_s
+
 #endif
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index d1c0656af8..eebfc9049f 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -40,19 +40,20 @@
 #pragma once
 
 #include <stdint.h>
+#include <unistd.h>
 
 namespace px4
 {
 
-void init(int argc, char *argv[], const char *process_name);
+__EXPORT void init(int argc, char *argv[], const char *process_name);
 
-uint64_t get_time_micros();
+__EXPORT uint64_t get_time_micros();
 
-bool ok();
+__EXPORT bool ok();
 
-void spin_once();
+__EXPORT void spin_once();
 
-void spin();
+__EXPORT void spin();
 
 class Rate
 {
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index fa2c8d6a47..34a6056476 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -39,8 +39,8 @@
 #pragma once
 
 /* includes for all platforms */
-#include <px4_subscriber.h>
-#include <px4_publisher.h>
+#include "px4_subscriber.h"
+#include "px4_publisher.h"
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /* includes when building for ros */
@@ -49,7 +49,6 @@
 #include <inttypes.h>
 #else
 /* includes when building for NuttX */
-#include <containers/List.hpp>
 #endif
 
 namespace px4
@@ -90,7 +89,7 @@ private:
 	std::list<Publisher> _pubs;
 };
 #else
-class NodeHandle
+class __EXPORT NodeHandle
 {
 public:
 	NodeHandle() :
@@ -101,20 +100,22 @@ public:
 	~NodeHandle() {};
 
 	template<typename M>
-	Subscriber subscribe(const char *topic, void(*fp)(M)) {
-		Subscriber sub(&_subs, , interval);
+	Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(M)) {
+		unsigned interval = 0;//XXX decide how to wrap this, ros equivalent?
+		//XXX
+		Subscriber *sub = new Subscriber(meta, interval, fp, &_subs);
 		return sub;
 	}
 
 	template<typename M>
-	Publisher advertise(const char *topic) {
-		Publisher pub(ros_pub);
-		_pubs.push_back(pub);
+	Publisher * advertise(const struct orb_metadata *meta) {
+		//XXX
+		Publisher * pub = new Publisher(meta, &_pubs);
 		return pub;
 	}
 private:
-	List<Subscriber> _subs;
-	List<Publisher> _pubs;
+	List<uORB::SubscriptionNode*> _subs;
+	List<uORB::PublicationNode*> _pubs;
 
 };
 #endif
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 53e63b695d..9ce211d250 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -43,6 +43,7 @@
 #else
 /* includes when building for NuttX */
 #include <uORB/Publication.hpp>
+#include <containers/List.hpp>
 #endif
 
 namespace px4
@@ -60,16 +61,24 @@ private:
 	ros::Publisher _ros_pub;
 };
 #else
-template<typename M>
 class Publisher :
-	public uORB::Publication<M>
+	public uORB::PublicationNode
+{
 public:
-	Publisher(List<SubscriptionBase *> * list,
-			const struct orb_metadata *meta, unsigned interval) :
-		uORB::Publication(list, meta)
+	Publisher(const struct orb_metadata *meta,
+			List<uORB::PublicationNode *> * list) :
+		uORB::PublicationNode(meta, list)
 	{}
 	~Publisher() {};
-{
+	template<typename M>
+	int publish(const M &msg) {
+		uORB::PublicationBase::update((void*)&msg);
+		return 0;
+	}
+
+	void update() {
+	//XXX list traversal callback, needed?
+	} ;
 };
 #endif
 }
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 6312e0cbea..12d422bb39 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -43,6 +43,7 @@
 #else
 /* includes when building for NuttX */
 #include <uORB/Subscription.hpp>
+#include <containers/List.hpp>
 #endif
 
 namespace px4
@@ -60,16 +61,23 @@ private:
 	ros::Subscriber _ros_sub;
 };
 #else
-template<typename M>
 class Subscriber :
-	public uORB::Subscription<M>
+	public uORB::SubscriptionNode
+{
 public:
-	Subscriber(List<SubscriptionBase *> * list,
-			const struct orb_metadata *meta, unsigned interval) :
-		uORB::Subsciption(list, meta, interval)
+	template<typename M>
+	Subscriber(const struct orb_metadata *meta,
+			unsigned interval,
+			void(*fp)(M),
+			List<uORB::SubscriptionNode *> * list) :
+		uORB::SubscriptionNode(meta, interval, list)
+		//XXX store callback
 	{}
 	~Subscriber() {};
-{
+
+	void update() {
+	//XXX list traversal callback, needed?
+	} ;
 };
 #endif
 

From 0474908e1c600d312934fcdd5790813813e799b1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 26 Nov 2014 11:55:41 +0100
Subject: [PATCH 072/416] reenable task flag

---
 src/examples/publisher/publisher.cpp   | 6 +++++-
 src/examples/subscriber/subscriber.cpp | 6 ++++--
 src/platforms/nuttx/px4_nodehandle.cpp | 5 +++++
 src/platforms/nuttx/px4_nuttx_impl.cpp | 5 +----
 src/platforms/px4_middleware.h         | 2 ++
 5 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 68fd8a5c3f..5ee8b459bc 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -32,7 +32,11 @@ using namespace px4;
 /**
  * This tutorial demonstrates simple sending of messages over the PX4 middleware system.
  */
-// __EXPORT bool task_should_exit;
+
+namespace px4
+{
+bool task_should_exit = false;
+}
 
 PX4_MAIN_FUNCTION(publisher)
 {
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index cdadaf2bcd..b872318809 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -36,8 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg)
 {
 	PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid);
 }
-
-// __EXPORT bool task_should_exit;
+namespace px4
+{
+bool task_should_exit = false;
+}
 
 PX4_MAIN_FUNCTION(subscriber)
 {
diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
index 473a5cf777..091b5c6c69 100644
--- a/src/platforms/nuttx/px4_nodehandle.cpp
+++ b/src/platforms/nuttx/px4_nodehandle.cpp
@@ -37,3 +37,8 @@
  * PX4 Middleware Wrapper Nodehandle
  */
 #include <platforms/px4_nodehandle.h>
+
+namespace px4
+{
+bool task_should_exit = false;
+}
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 4b87f68fee..6471e3e380 100644
--- a/src/platforms/nuttx/px4_nuttx_impl.cpp
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -40,7 +40,6 @@
 #include <px4.h>
 #include <drivers/drv_hrt.h>
 
-extern bool task_should_exit;
 
 namespace px4
 {
@@ -57,9 +56,7 @@ uint64_t get_time_micros()
 
 bool ok()
 {
-	// return !task_should_exit;
-	//XXX
-	return true;
+	return !task_should_exit;
 }
 
 void spin_once()
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index eebfc9049f..a09d9ac580 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -68,6 +68,8 @@ private:
 
 };
 
+extern bool task_should_exit;
+
 // /**
 //  * A limiter/ saturation.
 //  * The output of update is the input, bounded

From 818a49b5a81a5b821fa9a1a13db4591da5c33751 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 26 Nov 2014 12:45:03 +0100
Subject: [PATCH 073/416] fix ros compile errors

---
 src/examples/subscriber/subscriber.cpp |  4 ++--
 src/platforms/px4_nodehandle.h         | 12 ++++++------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index b872318809..093697d8e7 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -32,9 +32,9 @@ using namespace px4;
 /**
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  */
-void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg)
+void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
 {
-	PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid);
+	PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid);
 }
 namespace px4
 {
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 34a6056476..972792d536 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -69,24 +69,24 @@ public:
 	};
 
 	template<typename M>
-	Subscriber subscribe(const char *topic, void(*fp)(M)) {
+	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
-		Subscriber sub(ros_sub);
+		Subscriber * sub = new Subscriber(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
 
 	template<typename M>
-	Publisher advertise(const char *topic) {
+	Publisher * advertise(const char *topic) {
 		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
-		Publisher pub(ros_pub);
+		Publisher *pub =  new Publisher(ros_pub);
 		_pubs.push_back(pub);
 		return pub;
 	}
 private:
 	static const uint32_t kQueueSizeDefault = 1000;
-	std::list<Subscriber> _subs;
-	std::list<Publisher> _pubs;
+	std::list<Subscriber*> _subs;
+	std::list<Publisher*> _pubs;
 };
 #else
 class __EXPORT NodeHandle

From ee534b827a51a7356e565b665e280f765bd8d302 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 26 Nov 2014 13:18:28 +0100
Subject: [PATCH 074/416] move spin functions to nodehandle

---
 src/examples/publisher/publisher.cpp   |  2 +-
 src/examples/subscriber/subscriber.cpp |  2 +-
 src/platforms/px4_middleware.h         |  4 ----
 src/platforms/px4_nodehandle.h         |  5 +++++
 src/platforms/ros/px4_ros_impl.cpp     | 10 ----------
 5 files changed, 7 insertions(+), 16 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 5ee8b459bc..521c7c838a 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -90,7 +90,7 @@ PX4_MAIN_FUNCTION(publisher)
 		 */
 		rc_channels_pub->publish(msg);
 
-		px4::spin_once();
+		n.spinOnce();
 
 		loop_rate.sleep();
 		++count;
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 093697d8e7..0d0a81d7e0 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -85,7 +85,7 @@ PX4_MAIN_FUNCTION(subscriber)
 	 * callbacks will be called from within this thread (the main one).  px4::spin()
 	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
 	 */
-	px4::spin();
+	n.spin();
 
 	return 0;
 }
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index a09d9ac580..8a83f1af4a 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -51,10 +51,6 @@ __EXPORT uint64_t get_time_micros();
 
 __EXPORT bool ok();
 
-__EXPORT void spin_once();
-
-__EXPORT void spin();
-
 class Rate
 {
 
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 972792d536..b665f3fe79 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -53,6 +53,7 @@
 
 namespace px4
 {
+//XXX create abstract base class
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class NodeHandle :
 	private ros::NodeHandle
@@ -83,6 +84,10 @@ public:
 		_pubs.push_back(pub);
 		return pub;
 	}
+
+	void spin() { ros::spin(); }
+
+	void spinOnce() { ros::spinOnce(); }
 private:
 	static const uint32_t kQueueSizeDefault = 1000;
 	std::list<Subscriber*> _subs;
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
index eda17e5a91..3aa9761380 100644
--- a/src/platforms/ros/px4_ros_impl.cpp
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -58,14 +58,4 @@ bool ok()
 	return ros::ok();
 }
 
-void spin_once()
-{
-	ros::spinOnce();
-}
-
-void spin()
-{
-	ros::spin();
-}
-
 }

From 486d81cb95bb7dc912ee0a7636b0a6aeb87009d9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 08:12:46 +0100
Subject: [PATCH 075/416] NuttX submodule: header changes

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index ec6b670f6d..e3da98e379 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097
+Subproject commit e3da98e379452a1736c1098a44c000d2841956c0

From 36bf0c04c8cb052c67e613eb051b0deb650eb216 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 08:58:44 +0100
Subject: [PATCH 076/416] WIP, c++11 style callbacks for px4

---
 nuttx-configs/px4fmu-v2/nsh/Make.defs  |  4 +--
 src/examples/subscriber/subscriber.cpp | 10 +++++-
 src/platforms/nuttx/px4_nodehandle.cpp | 19 ++++++++++++
 src/platforms/nuttx/px4_nuttx_impl.cpp | 12 --------
 src/platforms/px4_nodehandle.h         | 13 ++++++--
 src/platforms/px4_subscriber.h         | 42 ++++++++++++++++++++------
 6 files changed, 74 insertions(+), 26 deletions(-)

diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
index f3ce53b4ae..5ae27aa008 100644
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -90,7 +90,7 @@ else
     ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
     ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
   else
-    # Linux/Cygwin-native toolchain 
+    # Linux/Cygwin-native toolchain
     MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
     ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
     ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
@@ -117,7 +117,7 @@ ARCHOPTIMIZATION	+= -g
 endif
 
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=c++11
 ARCHWARNINGS		 = -Wall \
 			   -Wextra \
 			   -Wdouble-promotion \
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 0d0a81d7e0..f8b1388443 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -36,6 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
 {
 	PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid);
 }
+// void rc_channels_callback(int i)
+// {
+	// PX4_INFO("I heard: [%d]", i);
+// }
 namespace px4
 {
 bool task_should_exit = false;
@@ -77,7 +81,11 @@ PX4_MAIN_FUNCTION(subscriber)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback);
+	// n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);});
+	// n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);});
+	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
+	std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
+	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
index 091b5c6c69..7930a06807 100644
--- a/src/platforms/nuttx/px4_nodehandle.cpp
+++ b/src/platforms/nuttx/px4_nodehandle.cpp
@@ -36,9 +36,28 @@
  *
  * PX4 Middleware Wrapper Nodehandle
  */
+#include <px4.h>
 #include <platforms/px4_nodehandle.h>
 
 namespace px4
 {
 bool task_should_exit = false;
+
+
+void NodeHandle::spinOnce() {
+	/* Loop through subscriptions, call callback for updated subscriptions */
+	uORB::SubscriptionNode *sub = _subs.getHead();
+	int count = 0;
+
+	while (sub != nullptr) {
+		if (count++ > kMaxSubscriptions) {
+			PX4_WARN("exceeded max subscriptions");
+			break;
+		}
+
+		sub->update();
+		sub = sub->getSibling();
+	}
+}
+
 }
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 6471e3e380..22daee8234 100644
--- a/src/platforms/nuttx/px4_nuttx_impl.cpp
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -59,16 +59,4 @@ bool ok()
 	return !task_should_exit;
 }
 
-void spin_once()
-{
-	// XXX check linked list of topics with orb_check() here
-
-}
-
-void spin()
-{
-	// XXX block waiting for updated topics here
-
-}
-
 }
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index b665f3fe79..f473bf629e 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -105,10 +105,12 @@ public:
 	~NodeHandle() {};
 
 	template<typename M>
-	Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(M)) {
+	Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) {
+	// Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(int i)> callback) {
+	// Subscriber * subscribe(const struct orb_metadata *meta, CallbackFunction callback) {
 		unsigned interval = 0;//XXX decide how to wrap this, ros equivalent?
 		//XXX
-		Subscriber *sub = new Subscriber(meta, interval, fp, &_subs);
+		Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs);
 		return sub;
 	}
 
@@ -118,7 +120,14 @@ public:
 		Publisher * pub = new Publisher(meta, &_pubs);
 		return pub;
 	}
+
+	void spinOnce();
+
+	void spin() {
+	 //XXX: call callbacks and do not return until task is terminated
+	}
 private:
+	static const uint16_t kMaxSubscriptions = 100;
 	List<uORB::SubscriptionNode*> _subs;
 	List<uORB::PublicationNode*> _pubs;
 
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 12d422bb39..71c3a766db 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -44,6 +44,7 @@
 /* includes when building for NuttX */
 #include <uORB/Subscription.hpp>
 #include <containers/List.hpp>
+#include <functional>
 #endif
 
 namespace px4
@@ -61,23 +62,46 @@ private:
 	ros::Subscriber _ros_sub;
 };
 #else
-class Subscriber :
-	public uORB::SubscriptionNode
+// typedef std::function<void(int)> CallbackFunction;
+class Subscriber
 {
 public:
-	template<typename M>
-	Subscriber(const struct orb_metadata *meta,
+	Subscriber() {};
+	~Subscriber() {};
+private:
+};
+
+template<typename M>
+class SubscriberPX4 :
+	public Subscriber,
+	public uORB::Subscription<M>
+{
+public:
+	SubscriberPX4(const struct orb_metadata *meta,
 			unsigned interval,
-			void(*fp)(M),
+			std::function<void(const M&)> callback,
+			// std::function<void(int i)> callback,
+			// CallbackFunction callback,
 			List<uORB::SubscriptionNode *> * list) :
-		uORB::SubscriptionNode(meta, interval, list)
+		Subscriber(),
+		uORB::Subscription<M>(meta, interval, list)
 		//XXX store callback
 	{}
-	~Subscriber() {};
+	~SubscriberPX4() {};
 
 	void update() {
-	//XXX list traversal callback, needed?
-	} ;
+		/* get latest data */
+		uORB::Subscription<M>::update();
+
+		/* Call callback which performs actions based on this data */
+		// _callback();
+
+	};
+private:
+	// std::function<void(int i)> _callback;
+	// CallbackFunction _callback;
+	 std::function<void(const M&)> _callback;
+
 };
 #endif
 

From 946d1203cfbbe522c61d6ed5dc1205c9f809f6f1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 09:45:39 +0100
Subject: [PATCH 077/416] remove unnecessary variable

---
 src/examples/subscriber/subscriber.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index f8b1388443..178fca4b5e 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -84,8 +84,10 @@ PX4_MAIN_FUNCTION(subscriber)
 	// n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);});
 	// n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);});
 	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
-	std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
-	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
+	// std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
+	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
+	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
+			[](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
 	PX4_INFO("subscribed");
 
 	/**

From bfc0a52ea23d18c9ef72690aa511e0feeb5a4176 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 09:48:12 +0100
Subject: [PATCH 078/416] update nuttx

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index e3da98e379..ae4b05e2c5 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit e3da98e379452a1736c1098a44c000d2841956c0
+Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c

From 59a9648bb60f8c33ca26b5f248d4b28dfc1de139 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 10:18:28 +0100
Subject: [PATCH 079/416] macro for topic subscription

---
 src/examples/subscriber/subscriber.cpp |  7 +++++--
 src/platforms/px4_defines.h            | 10 ++++++----
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 178fca4b5e..d6bc0bf377 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -86,8 +86,11 @@ PX4_MAIN_FUNCTION(subscriber)
 	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
 	// std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
 	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
-	n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
-			[](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
+	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
+			// [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
+	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
+			// [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index d4dc714536..79f3051402 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -48,8 +48,9 @@
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
-#define PX4_TOPIC(name) #name
-#define PX4_TOPIC_T(name) name
+#define PX4_TOPIC(_name) #_name
+#define PX4_TOPIC_T(_name) _name
+#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 
 #else
 /*
@@ -62,7 +63,8 @@
 #define PX4_WARN warnx
 #define PX4_WARN warnx
 #define PX4_INFO warnx
-#define PX4_TOPIC(name) ORB_ID(name)
-#define PX4_TOPIC_T(name) name##_s
+#define PX4_TOPIC(_name) ORB_ID(_name)
+#define PX4_TOPIC_T(_name) _name##_s
+#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), 	[](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);})
 
 #endif

From 244c1cb58387c5e8825abd4b27b4ff0b893f5383 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 11:34:52 +0100
Subject: [PATCH 080/416] enable systemcmds in test build

---
 makefiles/config_px4fmu-v2_test.mk | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 8aa8badc3c..844f1236a9 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -33,6 +33,25 @@ MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/ver
 
+#
+# System commands
+#
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
+
 #
 # Example modules
 #

From b351d67175e328ce3ab16642cdc69f6b8a239366 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 11:37:30 +0100
Subject: [PATCH 081/416] fix print for px4

---
 src/examples/publisher/publisher.cpp   | 3 +--
 src/examples/subscriber/subscriber.cpp | 3 ++-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 521c7c838a..5c7156c08e 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -80,7 +80,7 @@ PX4_MAIN_FUNCTION(publisher)
 		PX4_TOPIC_T(rc_channels) msg;
 
 		msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%lu", msg.timestamp_last_valid);
+		PX4_INFO("%llu", msg.timestamp_last_valid);
 
 		/**
 		 * The publish() function is how you send messages. The parameter
@@ -96,6 +96,5 @@ PX4_MAIN_FUNCTION(publisher)
 		++count;
 	}
 
-
 	return 0;
 }
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index d6bc0bf377..fa863463be 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -34,7 +34,7 @@ using namespace px4;
  */
 void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
 {
-	PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid);
+	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
 }
 // void rc_channels_callback(int i)
 // {
@@ -99,6 +99,7 @@ PX4_MAIN_FUNCTION(subscriber)
 	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
 	 */
 	n.spin();
+	PX4_INFO("finished, returning");
 
 	return 0;
 }

From 16f21d36dce17868f455318273140013febb6770 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 11:38:22 +0100
Subject: [PATCH 082/416] actually call callback

---
 src/platforms/px4_nodehandle.h |  6 +++---
 src/platforms/px4_subscriber.h | 11 ++++-------
 2 files changed, 7 insertions(+), 10 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index f473bf629e..e228dfca53 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -106,8 +106,6 @@ public:
 
 	template<typename M>
 	Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) {
-	// Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(int i)> callback) {
-	// Subscriber * subscribe(const struct orb_metadata *meta, CallbackFunction callback) {
 		unsigned interval = 0;//XXX decide how to wrap this, ros equivalent?
 		//XXX
 		Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs);
@@ -124,7 +122,9 @@ public:
 	void spinOnce();
 
 	void spin() {
-	 //XXX: call callbacks and do not return until task is terminated
+		while (true) { //XXX
+			spinOnce();
+		}
 	}
 private:
 	static const uint16_t kMaxSubscriptions = 100;
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 71c3a766db..efec8f2a39 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -80,11 +80,10 @@ public:
 	SubscriberPX4(const struct orb_metadata *meta,
 			unsigned interval,
 			std::function<void(const M&)> callback,
-			// std::function<void(int i)> callback,
-			// CallbackFunction callback,
 			List<uORB::SubscriptionNode *> * list) :
 		Subscriber(),
-		uORB::Subscription<M>(meta, interval, list)
+		uORB::Subscription<M>(meta, interval, list),
+		_callback(callback)
 		//XXX store callback
 	{}
 	~SubscriberPX4() {};
@@ -94,13 +93,11 @@ public:
 		uORB::Subscription<M>::update();
 
 		/* Call callback which performs actions based on this data */
-		// _callback();
+		_callback(uORB::Subscription<M>::getData());
 
 	};
 private:
-	// std::function<void(int i)> _callback;
-	// CallbackFunction _callback;
-	 std::function<void(const M&)> _callback;
+	std::function<void(const M&)> _callback;
 
 };
 #endif

From b0cfc2d1226c2b62a6bf3aac1809458b04902728 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 13:59:32 +0100
Subject: [PATCH 083/416] uORB::SubscriptionNode stores interval

---
 src/modules/uORB/Subscription.hpp | 27 +++++++++++++++++----------
 1 file changed, 17 insertions(+), 10 deletions(-)

diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index bab1ef2ea5..f82586285d 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -58,10 +58,10 @@ public:
 	/**
 	 * Constructor
 	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * @param meta The uORB metadata (usually from the ORB_ID()
 	 * 	macro) for the topic.
 	 *
-	 * @param interval  The minimum interval in milliseconds 
+	 * @param interval  The minimum interval in milliseconds
 	 * 	between updates
 	 */
 	SubscriptionBase(const struct orb_metadata *meta,
@@ -126,17 +126,18 @@ public:
 	 * Constructor
 	 *
 	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID() 
+	 * @param meta The uORB metadata (usually from the ORB_ID()
 	 * 	macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds 
+	 * @param interval  The minimum interval in milliseconds
 	 * 	between updates
-	 * @param list 	A pointer to a list of subscriptions 
+	 * @param list 	A pointer to a list of subscriptions
 	 * 	that this should be appended to.
 	 */
 	SubscriptionNode(const struct orb_metadata *meta,
 		unsigned interval=0,
 		List<SubscriptionNode *> * list=nullptr) :
-		SubscriptionBase(meta, interval) {
+		SubscriptionBase(meta, interval),
+		_interval(interval) {
 		if (list != nullptr) list->add(this);
 	}
 
@@ -145,6 +146,12 @@ public:
 	 * updates, a child class must implement it.
 	 */
 	virtual void update() = 0;
+// accessors
+	unsigned getInterval() { return _interval; }
+protected:
+// attributes
+	unsigned _interval;
+
 };
 
 /**
@@ -159,14 +166,14 @@ public:
 	/**
 	 * Constructor
 	 *
-	 * @param meta The uORB metadata (usually from 
+	 * @param meta The uORB metadata (usually from
 	 * 	the ORB_ID() macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds 
+	 * @param interval  The minimum interval in milliseconds
 	 * 	between updates
-	 * @param list A list interface for adding to 
+	 * @param list A list interface for adding to
 	 * 	list during construction
 	 */
-	Subscription(const struct orb_metadata *meta, 
+	Subscription(const struct orb_metadata *meta,
 		unsigned interval=0,
 		List<SubscriptionNode *> * list=nullptr);
 	/**

From 1b416a8e1f2b54e183d61a0022bb48de47576a6c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 14:00:02 +0100
Subject: [PATCH 084/416] use interval setting correctly, improve px4::spin

---
 src/examples/subscriber/subscriber.cpp | 11 +------
 src/platforms/px4_defines.h            |  4 +--
 src/platforms/px4_nodehandle.h         | 40 ++++++++++++++++++++------
 3 files changed, 35 insertions(+), 20 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index fa863463be..ed0635f180 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -81,16 +81,7 @@ PX4_MAIN_FUNCTION(subscriber)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	// n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);});
-	// n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);});
-	// CallbackFunction cbf =  [](int i){ return rc_channels_callback(i);};
-	// std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf =  [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);};
-	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf);
-	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
-			// [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
-	// n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels),
-			// [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);});
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback);
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 1000);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 79f3051402..7efe13bae3 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -50,7 +50,7 @@
 #define PX4_INFO ROS_INFO
 #define PX4_TOPIC(_name) #_name
 #define PX4_TOPIC_T(_name) _name
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 
 #else
 /*
@@ -65,6 +65,6 @@
 #define PX4_INFO warnx
 #define PX4_TOPIC(_name) ORB_ID(_name)
 #define PX4_TOPIC_T(_name) _name##_s
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), 	[](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);})
+#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval)
 
 #endif
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index e228dfca53..ced844fd5f 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -49,6 +49,7 @@
 #include <inttypes.h>
 #else
 /* includes when building for NuttX */
+#include <poll.h>
 #endif
 
 namespace px4
@@ -99,17 +100,23 @@ class __EXPORT NodeHandle
 public:
 	NodeHandle() :
 		_subs(),
-		_pubs()
+		_pubs(),
+		_sub_min_interval(nullptr)
 	{}
 
 	~NodeHandle() {};
 
 	template<typename M>
-	Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) {
-		unsigned interval = 0;//XXX decide how to wrap this, ros equivalent?
-		//XXX
-		Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs);
-		return sub;
+	Subscriber * subscribe(const struct orb_metadata *meta,
+			std::function<void(const M&)> callback,
+			unsigned interval) {
+		SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
+
+		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() < sub_px4->getInterval()) {
+			_sub_min_interval = sub_px4;
+		}
+		return (Subscriber*)sub_px4;
 	}
 
 	template<typename M>
@@ -122,7 +129,23 @@ public:
 	void spinOnce();
 
 	void spin() {
-		while (true) { //XXX
+		while (true) { //XXX check for termination
+			const int timeout_ms = 100;
+			/* Only continue in the loop if the nodehandle has subscriptions */
+			if (_sub_min_interval == nullptr) {
+				usleep(timeout_ms * 1000);
+				continue;
+			}
+
+			/* Poll fd with smallest interval */
+			struct pollfd pfd;
+			pfd.fd = _sub_min_interval->getHandle();
+			pfd.events = POLLIN;
+			if (poll(&pfd, 1, timeout_ms) <= 0) {
+				/* timed out */
+				continue;
+			}
+
 			spinOnce();
 		}
 	}
@@ -130,7 +153,8 @@ private:
 	static const uint16_t kMaxSubscriptions = 100;
 	List<uORB::SubscriptionNode*> _subs;
 	List<uORB::PublicationNode*> _pubs;
-
+	uORB::SubscriptionNode* _sub_min_interval;	/**< Points to the sub wtih the smallest interval
+							  of all Subscriptions in _subs*/
 };
 #endif
 }

From bc4209681c39fc934defcff318221e2c17c1ddb8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 14:05:24 +0100
Subject: [PATCH 085/416] remove comment

---
 src/platforms/px4_middleware.h | 27 ---------------------------
 1 file changed, 27 deletions(-)

diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index 8a83f1af4a..fa07b37666 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -65,30 +65,3 @@ private:
 };
 
 extern bool task_should_exit;
-
-// /**
-//  * A limiter/ saturation.
-//  * The output of update is the input, bounded
-//  * by min/max.
-//  */
-// class __EXPORT BlockLimit : public Block
-// {
-// public:
-// // methods
-// 	BlockLimit(SuperBlock *parent, const char *name) :
-// 		Block(parent, name),
-// 		_min(this, "MIN"),
-// 		_max(this, "MAX")
-// 	{};
-// 	virtual ~BlockLimit() {};
-// 	float update(float input);
-// // accessors
-// 	float getMin() { return _min.get(); }
-// 	float getMax() { return _max.get(); }
-// protected:
-// // attributes
-// 	control::BlockParamFloat _min;
-// 	control::BlockParamFloat _max;
-// };
-
-} // namespace px4

From cefccc0037f18275b8d8c7e49e15d13801ef28a1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 14:18:30 +0100
Subject: [PATCH 086/416] re-add accidentally deleted line from commit
 bc4209681c39fc934defcff318221e2c17c1ddb8

---
 src/platforms/px4_middleware.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index fa07b37666..462a06e97a 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -65,3 +65,5 @@ private:
 };
 
 extern bool task_should_exit;
+
+} // namespace px4

From a9c1e4ad6145485805366fad5c08ae7351886ff3 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 16:08:51 +0100
Subject: [PATCH 087/416] make px4::ok work, use it in px4::spin

---
 CMakeLists.txt                         |  3 ---
 src/platforms/nuttx/module.mk          |  5 +----
 src/platforms/nuttx/px4_nodehandle.cpp | 19 -------------------
 src/platforms/nuttx/px4_nuttx_impl.cpp |  5 -----
 src/platforms/px4_middleware.h         |  9 ++++++---
 src/platforms/px4_nodehandle.h         | 19 +++++++++++++++++--
 src/platforms/ros/px4_ros_impl.cpp     |  5 -----
 7 files changed, 24 insertions(+), 41 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8eb81c92ca..610284abee 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -102,9 +102,6 @@ include_directories(
 ## Declare a cpp library
 add_library(px4
   src/platforms/ros/px4_ros_impl.cpp
-  src/platforms/ros/px4_publisher.cpp
-  src/platforms/ros/px4_subscriber.cpp
-  src/platforms/ros/px4_nodehandle.cpp
 )
 
 target_link_libraries(px4
diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk
index 1c0ad7aa45..4a2aff8249 100644
--- a/src/platforms/nuttx/module.mk
+++ b/src/platforms/nuttx/module.mk
@@ -35,9 +35,6 @@
 # NuttX / uORB adapter library
 #
 
-SRCS		 = 	px4_nuttx_impl.cpp \
-			px4_publisher.cpp \
-			px4_subscriber.cpp \
-			px4_nodehandle.cpp
+SRCS		 = 	px4_nuttx_impl.cpp
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
index 7930a06807..ec557e8aa9 100644
--- a/src/platforms/nuttx/px4_nodehandle.cpp
+++ b/src/platforms/nuttx/px4_nodehandle.cpp
@@ -37,27 +37,8 @@
  * PX4 Middleware Wrapper Nodehandle
  */
 #include <px4.h>
-#include <platforms/px4_nodehandle.h>
 
 namespace px4
 {
-bool task_should_exit = false;
-
-
-void NodeHandle::spinOnce() {
-	/* Loop through subscriptions, call callback for updated subscriptions */
-	uORB::SubscriptionNode *sub = _subs.getHead();
-	int count = 0;
-
-	while (sub != nullptr) {
-		if (count++ > kMaxSubscriptions) {
-			PX4_WARN("exceeded max subscriptions");
-			break;
-		}
-
-		sub->update();
-		sub = sub->getSibling();
-	}
-}
 
 }
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 22daee8234..70e2923208 100644
--- a/src/platforms/nuttx/px4_nuttx_impl.cpp
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -54,9 +54,4 @@ uint64_t get_time_micros()
 	return hrt_absolute_time();
 }
 
-bool ok()
-{
-	return !task_should_exit;
-}
-
 }
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index 462a06e97a..dece729070 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -49,7 +49,12 @@ __EXPORT void init(int argc, char *argv[], const char *process_name);
 
 __EXPORT uint64_t get_time_micros();
 
-__EXPORT bool ok();
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+bool ok() { return ros::ok(); }
+#else
+extern bool task_should_exit;
+bool ok() { return !task_should_exit; }
+#endif
 
 class Rate
 {
@@ -64,6 +69,4 @@ private:
 
 };
 
-extern bool task_should_exit;
-
 } // namespace px4
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index ced844fd5f..eb90590e41 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -41,6 +41,7 @@
 /* includes for all platforms */
 #include "px4_subscriber.h"
 #include "px4_publisher.h"
+#include "px4_middleware.h"
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /* includes when building for ros */
@@ -126,10 +127,24 @@ public:
 		return pub;
 	}
 
-	void spinOnce();
+	void spinOnce() {
+		/* Loop through subscriptions, call callback for updated subscriptions */
+		uORB::SubscriptionNode *sub = _subs.getHead();
+		int count = 0;
+
+		while (sub != nullptr) {
+			if (count++ > kMaxSubscriptions) {
+				PX4_WARN("exceeded max subscriptions");
+				break;
+			}
+
+			sub->update();
+			sub = sub->getSibling();
+		}
+	}
 
 	void spin() {
-		while (true) { //XXX check for termination
+		while (ok()) {
 			const int timeout_ms = 100;
 			/* Only continue in the loop if the nodehandle has subscriptions */
 			if (_sub_min_interval == nullptr) {
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
index 3aa9761380..854986a7f3 100644
--- a/src/platforms/ros/px4_ros_impl.cpp
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -53,9 +53,4 @@ uint64_t get_time_micros()
 	return time.sec * 1e6 + time.nsec / 1000;
 }
 
-bool ok()
-{
-	return ros::ok();
-}
-
 }

From 9abc8e26b789af0ef132c5c38e3d8ada821c3657 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 16:30:12 +0100
Subject: [PATCH 088/416] correctly handle interval, call callback only when
 topic updated, add example for 2 topics

---
 src/examples/subscriber/subscriber.cpp | 11 ++++++-----
 src/platforms/px4_nodehandle.h         |  2 +-
 src/platforms/px4_subscriber.h         |  5 +++++
 3 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index ed0635f180..a186ba94a7 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -36,10 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
 {
 	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
 }
-// void rc_channels_callback(int i)
-// {
-	// PX4_INFO("I heard: [%d]", i);
-// }
+void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg)
+{
+	PX4_INFO("I heard2: [%llu]", msg.timestamp_last_valid);
+}
 namespace px4
 {
 bool task_should_exit = false;
@@ -81,7 +81,8 @@ PX4_MAIN_FUNCTION(subscriber)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 1000);
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index eb90590e41..45ef225fa6 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -114,7 +114,7 @@ public:
 		SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() < sub_px4->getInterval()) {
+		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
 			_sub_min_interval = sub_px4;
 		}
 		return (Subscriber*)sub_px4;
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index efec8f2a39..6e0ef8aed6 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -89,6 +89,11 @@ public:
 	~SubscriberPX4() {};
 
 	void update() {
+		if (!uORB::Subscription<M>::updated()) {
+			/* Topic not updated, do not call callback */
+			return;
+		}
+
 		/* get latest data */
 		uORB::Subscription<M>::update();
 

From caa61a4fdc7898987da7a03e1924ced8962bb92c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 23:09:45 +0100
Subject: [PATCH 089/416] add support for subcription method callbacks for ros
 and nuttx

---
 src/examples/subscriber/subscriber.cpp | 30 ++++++++++++++++++++------
 src/platforms/px4_defines.h            | 15 +++++++------
 src/platforms/px4_nodehandle.h         |  9 ++++++++
 3 files changed, 40 insertions(+), 14 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index a186ba94a7..3d54a1a2ff 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -32,21 +32,29 @@ using namespace px4;
 /**
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  */
-void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg)
-{
+void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
 }
-void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg)
-{
-	PX4_INFO("I heard2: [%llu]", msg.timestamp_last_valid);
+void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) {
+	PX4_INFO("I heard(2): [%llu]", msg.timestamp_last_valid);
 }
+
+class RCHandler {
+public:
+	void  callback(const PX4_TOPIC_T(rc_channels) &msg) {
+		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid);
+	}
+};
+
+RCHandler rchandler;
+
+
 namespace px4
 {
 bool task_should_exit = false;
 }
 
-PX4_MAIN_FUNCTION(subscriber)
-{
+PX4_MAIN_FUNCTION(subscriber) {
 	/**
 	 * The ros::init() function needs to see argc and argv so that it can perform
 	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
@@ -83,6 +91,14 @@ PX4_MAIN_FUNCTION(subscriber)
 	 */
 	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
 	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
+	//1
+	// PX4_SUBSCRIBE(n, rc_channels, callee.rc_channels_callback, , 1000);
+	//2
+	// PX4_SUBSCRIBE(n, rc_channels, rchandler.callback, &rchandler, 1000);
+	//3 for bind
+	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
+	// ros::NodeHandle n2;
+	// n2.subscribe("chatter", 1000, &RCHandler::callback, &rchandler);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 7efe13bae3..f6679d01b1 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -44,27 +44,28 @@
  * Building for running within the ROS environment
  */
 #define __EXPORT
-// #define PX4_MAIN_FUNCTION(_prefix)
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
 #define PX4_TOPIC(_name) #_name
 #define PX4_TOPIC_T(_name) _name
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 
 #else
 /*
  * Building for NuttX
  */
-
-// #define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##_main(int argc, char **argv)() { return main(argc, argv); }
-// #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) { return main(argc, argv); }
 #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
 #define PX4_WARN warnx
 #define PX4_WARN warnx
 #define PX4_INFO warnx
 #define PX4_TOPIC(_name) ORB_ID(_name)
 #define PX4_TOPIC_T(_name) _name##_s
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval)
-
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
 #endif
+
+/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
+#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
+#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 45ef225fa6..4153517565 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -71,6 +71,7 @@ public:
 		//XXX empty lists
 	};
 
+	/* Constructor with callback to function */
 	template<typename M>
 	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
@@ -78,6 +79,14 @@ public:
 		_subs.push_back(sub);
 		return sub;
 	}
+	/* Constructor with callback to class method */
+	template<typename M, typename T>
+	Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
+		Subscriber * sub = new Subscriber(ros_sub);
+		_subs.push_back(sub);
+		return sub;
+	}
 
 	template<typename M>
 	Publisher * advertise(const char *topic) {

From 6b695ac9e8be9e7fe480238c967316366cba444c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 23:14:50 +0100
Subject: [PATCH 090/416] add PX4 advertise macro

---
 src/examples/publisher/publisher.cpp | 2 +-
 src/platforms/px4_defines.h          | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 5c7156c08e..8ef147493f 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -62,7 +62,7 @@ PX4_MAIN_FUNCTION(publisher)
 	 * than we can send them, the number here specifies how many messages to
 	 * buffer up before throwing some away.
 	 */
-	px4::Publisher * rc_channels_pub = n.advertise<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels));
+	px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels);
 
 
 	px4::Rate loop_rate(10);
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index f6679d01b1..84e5c8da09 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -69,3 +69,4 @@
 /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
 #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
 #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
+#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))

From 8b5bc703a11805cada41e06b5e2327d0796ec0e5 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Dec 2014 16:39:27 +0100
Subject: [PATCH 091/416] initial version of msg to uorb script

Standard and embedded types work, may need small refinements for some
types
---
 .gitignore                              |   1 +
 Makefile                                |  20 +++-
 Tools/px_generate_uorb_topic_headers.py |  98 ++++++++++++++++
 msg/px4_msgs/actuator_armed.msg         |   6 +
 msg/px4_msgs/rc_channels.msg            |  24 ++++
 msg/rc_channels.msg                     |   8 --
 msg/std_msgs/Bool.msg                   |   1 +
 msg/std_msgs/Byte.msg                   |   1 +
 msg/std_msgs/ByteMultiArray.msg         |   6 +
 msg/std_msgs/Char.msg                   |   1 +
 msg/std_msgs/ColorRGBA.msg              |   4 +
 msg/std_msgs/Duration.msg               |   1 +
 msg/std_msgs/Empty.msg                  |   0
 msg/std_msgs/Float32.msg                |   1 +
 msg/std_msgs/Float32MultiArray.msg      |   6 +
 msg/std_msgs/Float64.msg                |   1 +
 msg/std_msgs/Float64MultiArray.msg      |   6 +
 msg/std_msgs/Header.msg                 |  15 +++
 msg/std_msgs/Int16.msg                  |   1 +
 msg/std_msgs/Int16MultiArray.msg        |   6 +
 msg/std_msgs/Int32.msg                  |   1 +
 msg/std_msgs/Int32MultiArray.msg        |   6 +
 msg/std_msgs/Int64.msg                  |   1 +
 msg/std_msgs/Int64MultiArray.msg        |   6 +
 msg/std_msgs/Int8.msg                   |   1 +
 msg/std_msgs/Int8MultiArray.msg         |   6 +
 msg/std_msgs/MultiArrayDimension.msg    |   3 +
 msg/std_msgs/MultiArrayLayout.msg       |  26 +++++
 msg/std_msgs/String.msg                 |   1 +
 msg/std_msgs/Time.msg                   |   1 +
 msg/std_msgs/UInt16.msg                 |   1 +
 msg/std_msgs/UInt16MultiArray.msg       |   6 +
 msg/std_msgs/UInt32.msg                 |   1 +
 msg/std_msgs/UInt32MultiArray.msg       |   6 +
 msg/std_msgs/UInt64.msg                 |   1 +
 msg/std_msgs/UInt64MultiArray.msg       |   6 +
 msg/std_msgs/UInt8.msg                  |   1 +
 msg/std_msgs/UInt8MultiArray.msg        |   6 +
 msg/templates/msg.h.template            | 141 ++++++++++++++++++++++++
 39 files changed, 415 insertions(+), 13 deletions(-)
 create mode 100755 Tools/px_generate_uorb_topic_headers.py
 create mode 100644 msg/px4_msgs/actuator_armed.msg
 create mode 100644 msg/px4_msgs/rc_channels.msg
 delete mode 100644 msg/rc_channels.msg
 create mode 100644 msg/std_msgs/Bool.msg
 create mode 100644 msg/std_msgs/Byte.msg
 create mode 100644 msg/std_msgs/ByteMultiArray.msg
 create mode 100644 msg/std_msgs/Char.msg
 create mode 100644 msg/std_msgs/ColorRGBA.msg
 create mode 100644 msg/std_msgs/Duration.msg
 create mode 100644 msg/std_msgs/Empty.msg
 create mode 100644 msg/std_msgs/Float32.msg
 create mode 100644 msg/std_msgs/Float32MultiArray.msg
 create mode 100644 msg/std_msgs/Float64.msg
 create mode 100644 msg/std_msgs/Float64MultiArray.msg
 create mode 100644 msg/std_msgs/Header.msg
 create mode 100644 msg/std_msgs/Int16.msg
 create mode 100644 msg/std_msgs/Int16MultiArray.msg
 create mode 100644 msg/std_msgs/Int32.msg
 create mode 100644 msg/std_msgs/Int32MultiArray.msg
 create mode 100644 msg/std_msgs/Int64.msg
 create mode 100644 msg/std_msgs/Int64MultiArray.msg
 create mode 100644 msg/std_msgs/Int8.msg
 create mode 100644 msg/std_msgs/Int8MultiArray.msg
 create mode 100644 msg/std_msgs/MultiArrayDimension.msg
 create mode 100644 msg/std_msgs/MultiArrayLayout.msg
 create mode 100644 msg/std_msgs/String.msg
 create mode 100644 msg/std_msgs/Time.msg
 create mode 100644 msg/std_msgs/UInt16.msg
 create mode 100644 msg/std_msgs/UInt16MultiArray.msg
 create mode 100644 msg/std_msgs/UInt32.msg
 create mode 100644 msg/std_msgs/UInt32MultiArray.msg
 create mode 100644 msg/std_msgs/UInt64.msg
 create mode 100644 msg/std_msgs/UInt64MultiArray.msg
 create mode 100644 msg/std_msgs/UInt8.msg
 create mode 100644 msg/std_msgs/UInt8MultiArray.msg
 create mode 100644 msg/templates/msg.h.template

diff --git a/.gitignore b/.gitignore
index 69112ee1fa..9d9c3d3833 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,3 +39,4 @@ tags
 .pydevproject
 .ropeproject
 *.orig
+src/modules/uORB/topics/*
diff --git a/Makefile b/Makefile
index 809f54dd31..7df8004a40 100644
--- a/Makefile
+++ b/Makefile
@@ -104,13 +104,13 @@ DESIRED_FIRMWARES 	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
 STAGED_FIRMWARES	 = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
 FIRMWARES		 = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
 
-all:			checksubmodules $(DESIRED_FIRMWARES)
+all:	checksubmodules generateuorbtopicheaders $(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.
 #
-# XXX copying the .bin files is a hack to work around the PX4IO uploader 
-#     not supporting .px4 files, and it should be deprecated onced that 
+# XXX copying the .bin files is a hack to work around the PX4IO uploader
+#     not supporting .px4 files, and it should be deprecated onced that
 #     is taken care of.
 #
 $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
@@ -152,7 +152,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
 # Build the NuttX export archives.
 #
 # Note that there are no explicit dependencies extended from these
-# archives. If NuttX is updated, the user is expected to rebuild the 
+# archives. If NuttX is updated, the user is expected to rebuild the
 # archives/build area manually. Likewise, when the 'archives' target is
 # invoked, all archives are always rebuilt.
 #
@@ -224,6 +224,16 @@ updatesubmodules:
 	$(Q) (git submodule init)
 	$(Q) (git submodule update)
 
+MSG_DIR = $(PX4_BASE)msg/px4_msgs
+MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
+TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
+
+.PHONY: generateuorbtopicheaders
+generateuorbtopicheaders:
+	@$(ECHO) "Generating uORB topic headers"
+	$(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \
+		-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR))
+
 #
 # Testing targets
 #
@@ -232,7 +242,7 @@ testbuild:
 
 #
 # Cleanup targets.  'clean' should remove all built products and force
-# a complete re-compilation, 'distclean' should remove everything 
+# a complete re-compilation, 'distclean' should remove everything
 # that's generated leaving only files that are in source control.
 #
 .PHONY:	clean
diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
new file mode 100755
index 0000000000..a738dcb7ef
--- /dev/null
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+#############################################################################
+#
+#   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+
+"""
+px_generate_uorb_topic_headers.py
+Generates c/cpp header files for uorb topics from .msg (ROS syntax)
+message files
+"""
+from __future__ import print_function
+import os
+import argparse
+import genmsg.template_tools
+
+__author__ = "Thomas Gubler"
+__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
+__license__ = "BSD"
+__email__ = "thomasgubler@gmail.com"
+
+
+msg_template_map = {'msg.h.template': '@NAME@.h'}
+srv_template_map = {}
+incl_default = ['std_msgs:./msg/std_msgs']
+package = 'px4'
+
+
+def convert_file(filename, outputdir, templatedir, includepath):
+    """
+    Converts a single .msg file to a uorb header
+    """
+    print("Generating uORB headers from {0}".format(filename))
+    genmsg.template_tools.generate_from_file(filename,
+                                             package,
+                                             outputdir,
+                                             templatedir,
+                                             includepath,
+                                             msg_template_map,
+                                             srv_template_map)
+
+
+def convert_dir(inputdir, outputdir, templatedir):
+    """
+    Converts all .msg files in inputdir to uORB header files
+    """
+    includepath = incl_default + [':'.join([package, inputdir])]
+    for f in os.listdir(inputdir):
+        fn = os.path.join(inputdir, f)
+        if os.path.isfile(fn):
+            convert_file(fn, outputdir, templatedir, includepath)
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description='Convert msg files to uorb headers')
+    parser.add_argument('-d', dest='dir', help='directory with msg files')
+    parser.add_argument('-f', dest='file',
+                        help="files to convert (use only without -d)",
+                        nargs="+")
+    parser.add_argument('-e', dest='templatedir',
+                        help='directory with template files',)
+    parser.add_argument('-o', dest='outputdir',
+                        help='output directory for header files')
+    args = parser.parse_args()
+
+    if args.file is not None:
+        for f in args.file:
+            convert_file(f, args.outputdir, args.templatedir, incl_default)
+    elif args.dir is not None:
+            convert_dir(args.dir, args.outputdir, args.templatedir)
diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg
new file mode 100644
index 0000000000..f6bf583073
--- /dev/null
+++ b/msg/px4_msgs/actuator_armed.msg
@@ -0,0 +1,6 @@
+
+uint64 timestamp	# Microseconds since system boot
+bool armed          # Set to true if system is armed
+bool ready_to_arm	# Set to true if system is ready to be armed
+bool lockdown	    # Set to true if actuators are forced to being disabled (due to emergency or HIL)
+bool force_failsafe # Set to true if the actuators are forced to the failsafe position
diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg
new file mode 100644
index 0000000000..aaa6696f9b
--- /dev/null
+++ b/msg/px4_msgs/rc_channels.msg
@@ -0,0 +1,24 @@
+int32 RC_CHANNELS_FUNCTION_MAX=18
+uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
+uint8 RC_CHANNELS_FUNCTION_ROLL=1
+uint8 RC_CHANNELS_FUNCTION_PITCH=2
+uint8 RC_CHANNELS_FUNCTION_YAW=3
+uint8 RC_CHANNELS_FUNCTION_MODE=4
+uint8 RC_CHANNELS_FUNCTION_RETURN=5
+uint8 RC_CHANNELS_FUNCTION_POSCTL=6
+uint8 RC_CHANNELS_FUNCTION_LOITER=7
+uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8
+uint8 RC_CHANNELS_FUNCTION_ACRO=9
+uint8 RC_CHANNELS_FUNCTION_FLAPS=10
+uint8 RC_CHANNELS_FUNCTION_AUX_1=11
+uint8 RC_CHANNELS_FUNCTION_AUX_2=12
+uint8 RC_CHANNELS_FUNCTION_AUX_3=13
+uint8 RC_CHANNELS_FUNCTION_AUX_4=14
+uint8 RC_CHANNELS_FUNCTION_AUX_5=15
+uint64 timestamp_last_valid					# Timestamp of last valid RC signal
+float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
+uint8 channel_count						# Number of valid channels
+int8[18] function						# Functions mapping
+uint8 rssi							# Receive signal strength index
+bool signal_lost						# Control signal lost, should be checked together with topic timeout
+actuator_armed actuator
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
deleted file mode 100644
index 11dc57c42b..0000000000
--- a/msg/rc_channels.msg
+++ /dev/null
@@ -1,8 +0,0 @@
-Header header
-int32 RC_CHANNELS_FUNCTION_MAX=18
-uint64 timestamp_last_valid					# Timestamp of last valid RC signal
-float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
-uint8 channel_count						# Number of valid channels
-int8[18] function						# Functions mapping
-uint8 rssi							# Receive signal strength index
-bool signal_lost						# Control signal lost, should be checked together with topic timeout
diff --git a/msg/std_msgs/Bool.msg b/msg/std_msgs/Bool.msg
new file mode 100644
index 0000000000..f7cabb94fc
--- /dev/null
+++ b/msg/std_msgs/Bool.msg
@@ -0,0 +1 @@
+bool data
\ No newline at end of file
diff --git a/msg/std_msgs/Byte.msg b/msg/std_msgs/Byte.msg
new file mode 100644
index 0000000000..d993b34555
--- /dev/null
+++ b/msg/std_msgs/Byte.msg
@@ -0,0 +1 @@
+byte data
diff --git a/msg/std_msgs/ByteMultiArray.msg b/msg/std_msgs/ByteMultiArray.msg
new file mode 100644
index 0000000000..bb00bd348b
--- /dev/null
+++ b/msg/std_msgs/ByteMultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+byte[]            data          # array of data
+
diff --git a/msg/std_msgs/Char.msg b/msg/std_msgs/Char.msg
new file mode 100644
index 0000000000..39a1d46a97
--- /dev/null
+++ b/msg/std_msgs/Char.msg
@@ -0,0 +1 @@
+char data
\ No newline at end of file
diff --git a/msg/std_msgs/ColorRGBA.msg b/msg/std_msgs/ColorRGBA.msg
new file mode 100644
index 0000000000..182dbc8349
--- /dev/null
+++ b/msg/std_msgs/ColorRGBA.msg
@@ -0,0 +1,4 @@
+float32 r
+float32 g
+float32 b
+float32 a
diff --git a/msg/std_msgs/Duration.msg b/msg/std_msgs/Duration.msg
new file mode 100644
index 0000000000..f13931ec8a
--- /dev/null
+++ b/msg/std_msgs/Duration.msg
@@ -0,0 +1 @@
+duration data
diff --git a/msg/std_msgs/Empty.msg b/msg/std_msgs/Empty.msg
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/msg/std_msgs/Float32.msg b/msg/std_msgs/Float32.msg
new file mode 100644
index 0000000000..e89740534b
--- /dev/null
+++ b/msg/std_msgs/Float32.msg
@@ -0,0 +1 @@
+float32 data
\ No newline at end of file
diff --git a/msg/std_msgs/Float32MultiArray.msg b/msg/std_msgs/Float32MultiArray.msg
new file mode 100644
index 0000000000..915830846d
--- /dev/null
+++ b/msg/std_msgs/Float32MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+float32[]         data          # array of data
+
diff --git a/msg/std_msgs/Float64.msg b/msg/std_msgs/Float64.msg
new file mode 100644
index 0000000000..cd09d39b8c
--- /dev/null
+++ b/msg/std_msgs/Float64.msg
@@ -0,0 +1 @@
+float64 data
\ No newline at end of file
diff --git a/msg/std_msgs/Float64MultiArray.msg b/msg/std_msgs/Float64MultiArray.msg
new file mode 100644
index 0000000000..0a13b928fd
--- /dev/null
+++ b/msg/std_msgs/Float64MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+float64[]         data          # array of data
+
diff --git a/msg/std_msgs/Header.msg b/msg/std_msgs/Header.msg
new file mode 100644
index 0000000000..f90d622ea5
--- /dev/null
+++ b/msg/std_msgs/Header.msg
@@ -0,0 +1,15 @@
+# Standard metadata for higher-level stamped data types.
+# This is generally used to communicate timestamped data 
+# in a particular coordinate frame.
+# 
+# sequence ID: consecutively increasing ID 
+uint32 seq
+#Two-integer timestamp that is expressed as:
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
+# time-handling sugar is provided by the client library
+time stamp
+#Frame this data is associated with
+# 0: no frame
+# 1: global frame
+string frame_id
diff --git a/msg/std_msgs/Int16.msg b/msg/std_msgs/Int16.msg
new file mode 100644
index 0000000000..c4389faf70
--- /dev/null
+++ b/msg/std_msgs/Int16.msg
@@ -0,0 +1 @@
+int16 data
diff --git a/msg/std_msgs/Int16MultiArray.msg b/msg/std_msgs/Int16MultiArray.msg
new file mode 100644
index 0000000000..d2ddea1d1d
--- /dev/null
+++ b/msg/std_msgs/Int16MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+int16[]           data          # array of data
+
diff --git a/msg/std_msgs/Int32.msg b/msg/std_msgs/Int32.msg
new file mode 100644
index 0000000000..0ecfe35f5f
--- /dev/null
+++ b/msg/std_msgs/Int32.msg
@@ -0,0 +1 @@
+int32 data
\ No newline at end of file
diff --git a/msg/std_msgs/Int32MultiArray.msg b/msg/std_msgs/Int32MultiArray.msg
new file mode 100644
index 0000000000..af60abda3a
--- /dev/null
+++ b/msg/std_msgs/Int32MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+int32[]           data          # array of data
+
diff --git a/msg/std_msgs/Int64.msg b/msg/std_msgs/Int64.msg
new file mode 100644
index 0000000000..6961e00f52
--- /dev/null
+++ b/msg/std_msgs/Int64.msg
@@ -0,0 +1 @@
+int64 data
\ No newline at end of file
diff --git a/msg/std_msgs/Int64MultiArray.msg b/msg/std_msgs/Int64MultiArray.msg
new file mode 100644
index 0000000000..f4f35e171b
--- /dev/null
+++ b/msg/std_msgs/Int64MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+int64[]           data          # array of data
+
diff --git a/msg/std_msgs/Int8.msg b/msg/std_msgs/Int8.msg
new file mode 100644
index 0000000000..1e42e554fe
--- /dev/null
+++ b/msg/std_msgs/Int8.msg
@@ -0,0 +1 @@
+int8 data
diff --git a/msg/std_msgs/Int8MultiArray.msg b/msg/std_msgs/Int8MultiArray.msg
new file mode 100644
index 0000000000..a59a37259e
--- /dev/null
+++ b/msg/std_msgs/Int8MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+int8[]            data          # array of data
+
diff --git a/msg/std_msgs/MultiArrayDimension.msg b/msg/std_msgs/MultiArrayDimension.msg
new file mode 100644
index 0000000000..08240462c4
--- /dev/null
+++ b/msg/std_msgs/MultiArrayDimension.msg
@@ -0,0 +1,3 @@
+string label   # label of given dimension
+uint32 size    # size of given dimension (in type units)
+uint32 stride  # stride of given dimension
\ No newline at end of file
diff --git a/msg/std_msgs/MultiArrayLayout.msg b/msg/std_msgs/MultiArrayLayout.msg
new file mode 100644
index 0000000000..5437f8542a
--- /dev/null
+++ b/msg/std_msgs/MultiArrayLayout.msg
@@ -0,0 +1,26 @@
+# The multiarray declares a generic multi-dimensional array of a
+# particular data type.  Dimensions are ordered from outer most
+# to inner most.
+
+MultiArrayDimension[] dim # Array of dimension properties
+uint32 data_offset        # padding bytes at front of data
+
+# Accessors should ALWAYS be written in terms of dimension stride
+# and specified outer-most dimension first.
+# 
+# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]
+#
+# A standard, 3-channel 640x480 image with interleaved color channels
+# would be specified as:
+#
+# dim[0].label  = "height"
+# dim[0].size   = 480
+# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)
+# dim[1].label  = "width"
+# dim[1].size   = 640
+# dim[1].stride = 3*640 = 1920
+# dim[2].label  = "channel"
+# dim[2].size   = 3
+# dim[2].stride = 3
+#
+# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.
\ No newline at end of file
diff --git a/msg/std_msgs/String.msg b/msg/std_msgs/String.msg
new file mode 100644
index 0000000000..ae721739e8
--- /dev/null
+++ b/msg/std_msgs/String.msg
@@ -0,0 +1 @@
+string data
diff --git a/msg/std_msgs/Time.msg b/msg/std_msgs/Time.msg
new file mode 100644
index 0000000000..7f8f721711
--- /dev/null
+++ b/msg/std_msgs/Time.msg
@@ -0,0 +1 @@
+time data
diff --git a/msg/std_msgs/UInt16.msg b/msg/std_msgs/UInt16.msg
new file mode 100644
index 0000000000..87d0c44eb5
--- /dev/null
+++ b/msg/std_msgs/UInt16.msg
@@ -0,0 +1 @@
+uint16 data
diff --git a/msg/std_msgs/UInt16MultiArray.msg b/msg/std_msgs/UInt16MultiArray.msg
new file mode 100644
index 0000000000..f38970b656
--- /dev/null
+++ b/msg/std_msgs/UInt16MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+uint16[]            data        # array of data
+
diff --git a/msg/std_msgs/UInt32.msg b/msg/std_msgs/UInt32.msg
new file mode 100644
index 0000000000..b6c696b421
--- /dev/null
+++ b/msg/std_msgs/UInt32.msg
@@ -0,0 +1 @@
+uint32 data
\ No newline at end of file
diff --git a/msg/std_msgs/UInt32MultiArray.msg b/msg/std_msgs/UInt32MultiArray.msg
new file mode 100644
index 0000000000..b2bb0771f0
--- /dev/null
+++ b/msg/std_msgs/UInt32MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+uint32[]          data          # array of data
+
diff --git a/msg/std_msgs/UInt64.msg b/msg/std_msgs/UInt64.msg
new file mode 100644
index 0000000000..2eb1afad37
--- /dev/null
+++ b/msg/std_msgs/UInt64.msg
@@ -0,0 +1 @@
+uint64 data
\ No newline at end of file
diff --git a/msg/std_msgs/UInt64MultiArray.msg b/msg/std_msgs/UInt64MultiArray.msg
new file mode 100644
index 0000000000..30d0cd9285
--- /dev/null
+++ b/msg/std_msgs/UInt64MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+uint64[]          data          # array of data
+
diff --git a/msg/std_msgs/UInt8.msg b/msg/std_msgs/UInt8.msg
new file mode 100644
index 0000000000..5eefd870db
--- /dev/null
+++ b/msg/std_msgs/UInt8.msg
@@ -0,0 +1 @@
+uint8 data
diff --git a/msg/std_msgs/UInt8MultiArray.msg b/msg/std_msgs/UInt8MultiArray.msg
new file mode 100644
index 0000000000..31f7d6a213
--- /dev/null
+++ b/msg/std_msgs/UInt8MultiArray.msg
@@ -0,0 +1,6 @@
+# Please look at the MultiArrayLayout message definition for
+# documentation on all multiarrays.
+
+MultiArrayLayout  layout        # specification of data layout
+uint8[]           data          # array of data
+
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
new file mode 100644
index 0000000000..d05282c885
--- /dev/null
+++ b/msg/templates/msg.h.template
@@ -0,0 +1,141 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace
+cpp_class = '%s_'%spec.short_name
+cpp_full_name = '%s%s'%(cpp_namespace,cpp_class)
+cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name)
+cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <stdint.h>
+#include "../uORB.h"
+
+@##############################
+@# Includes for dependencies
+@##############################
+@{
+for field in spec.parsed_fields():
+  if (not field.is_builtin):
+    if (not field.is_header):
+      (package, name) = genmsg.names.package_resource_name(field.base_type)
+      package = package or spec.package # convert '' to package
+      print('#include <uORB/topics/%s.h>'%(name))
+
+}@
+@# Constants
+@[for constant in spec.constants]@
+#define @(constant.name) = @(int(constant.val))
+@[end for]
+
+/**
+ * @@addtogroup topics
+ * @@{
+ */
+
+@##############################
+@# Main struct of message
+@##############################
+@{
+
+inttypes = ['int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64']
+
+# Function to print a standard ros type
+def print_field_def(field):
+  type = field.type
+  # detect embedded types
+  sl_pos = type.find('/')
+  type_appendix = ''
+  type_prefix = ''
+  if (sl_pos >= 0):
+    type = type[sl_pos + 1:]
+    type_prefix = 'struct '
+    type_appendix = '_s'
+
+  # detect arrays
+  a_pos = type.find('[')
+  array_size = ''
+  if (a_pos >= 0):
+    # field is array
+    array_size = type[a_pos:]
+    type = type[:a_pos]
+
+  if type in inttypes:
+    # need to add _t: int8 --> int8_t
+    type_appendix = '_t'
+
+  print('\t%s%s%s%s %s;'%(type_prefix, type, type_appendix, array_size, field.name))
+
+}
+struct @(spec.short_name)_s
+@{
+# loop over all fields and print the type and name
+for field in spec.parsed_fields():
+  if (not field.is_header):
+    print_field_def(field)
+}@
+}
+
+/**
+ * @@}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(@(spec.short_name));

From 6a2fcb8874ba996fb7f5914376197a38b7e73459 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Dec 2014 17:44:15 +0100
Subject: [PATCH 092/416] move (for now) unused msg file

---
 msg/px4_msgs/{ => unused}/actuator_armed.msg | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename msg/px4_msgs/{ => unused}/actuator_armed.msg (100%)

diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg
similarity index 100%
rename from msg/px4_msgs/actuator_armed.msg
rename to msg/px4_msgs/unused/actuator_armed.msg

From 2eeeab8ecd04cdbf4ef80161a1708c140a9d8ba6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Dec 2014 17:45:10 +0100
Subject: [PATCH 093/416] improve msg template file

---
 msg/templates/msg.h.template | 23 +++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index d05282c885..f60001b55d 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -95,7 +95,16 @@ for field in spec.parsed_fields():
 @##############################
 @{
 
-inttypes = ['int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64']
+type_map = {'int8': 'int8_t',
+  'int16': 'int16_t',
+  'int32': 'int32_t',
+  'int64': 'int64_t',
+  'uint8': 'uint8_t',
+  'uint16': 'uint16_t',
+  'uint32': 'uint32_t',
+  'uint64': 'uint64_t',
+  'float32': 'float',
+  'bool': 'bool'}
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -117,21 +126,23 @@ def print_field_def(field):
     array_size = type[a_pos:]
     type = type[:a_pos]
 
-  if type in inttypes:
+  if type in type_map:
     # need to add _t: int8 --> int8_t
-    type_appendix = '_t'
+    type_px4 = type_map[type]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
 
-  print('\t%s%s%s%s %s;'%(type_prefix, type, type_appendix, array_size, field.name))
+  print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
 
 }
-struct @(spec.short_name)_s
+struct @(spec.short_name)_s {
 @{
 # loop over all fields and print the type and name
 for field in spec.parsed_fields():
   if (not field.is_header):
     print_field_def(field)
 }@
-}
+};
 
 /**
  * @@}

From 3856271abb0b90aba62f35668513463c6555734b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Dec 2014 17:46:25 +0100
Subject: [PATCH 094/416] remove embedded message test from rc_channels.msg

---
 msg/px4_msgs/rc_channels.msg | 1 -
 1 file changed, 1 deletion(-)

diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg
index aaa6696f9b..19fd14c7cc 100644
--- a/msg/px4_msgs/rc_channels.msg
+++ b/msg/px4_msgs/rc_channels.msg
@@ -21,4 +21,3 @@ uint8 channel_count						# Number of valid channels
 int8[18] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
-actuator_armed actuator

From dfb266565a2f1849ddd67460950b674a44eb6530 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 10:45:42 +0100
Subject: [PATCH 095/416] update ros configuration for new file locations

---
 CMakeLists.txt                    |  4 +++-
 msg/px4_msgs/vehicle_attitude.msg | 18 ++++++++++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)
 create mode 100644 msg/px4_msgs/vehicle_attitude.msg

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 610284abee..636cc07e34 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -47,7 +47,9 @@ find_package(catkin REQUIRED COMPONENTS
 ## Generate messages in the 'msg' folder
 add_message_files(
   FILES
-  rc_channels.msg
+  px4_msgs/rc_channels.msg
+  px4_msgs/vehicle_attitude.msg
+  px4_msgs/rc_channels.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/msg/px4_msgs/vehicle_attitude.msg b/msg/px4_msgs/vehicle_attitude.msg
new file mode 100644
index 0000000000..98018a1df0
--- /dev/null
+++ b/msg/px4_msgs/vehicle_attitude.msg
@@ -0,0 +1,18 @@
+# This is similar to the mavlink message ATTITUDE, but for onboard use */
+uint64 timestamp	# in microseconds since system start
+# @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional
+float32 roll		# Roll angle (rad, Tait-Bryan, NED)
+float32 pitch		# Pitch angle (rad, Tait-Bryan, NED)
+float32 yaw		# Yaw angle (rad, Tait-Bryan, NED)
+float32 rollspeed	# Roll angular speed (rad/s, Tait-Bryan, NED)
+float32 pitchspeed	# Pitch angular speed (rad/s, Tait-Bryan, NED)
+float32 yawspeed	# Yaw angular speed (rad/s, Tait-Bryan, NED)
+float32 rollacc		# Roll angular accelration (rad/s, Tait-Bryan, NED)
+float32 pitchacc	# Pitch angular acceleration (rad/s, Tait-Bryan, NED)
+float32 yawacc		# Yaw angular acceleration (rad/s, Tait-Bryan, NED)
+float32[3] rate_offsets	# Offsets of the body angular rates from zero
+float32[9] R		# Rotation matrix, body to world, (Tait-Bryan, NED)
+float32[4] q		# Quaternion (NED)
+float32[3] g_comp		# Compensated gravity vector
+bool R_valid		# Rotation matrix valid
+bool q_valid		# Quaternion valid

From 4d91c61f8f5aaebd8bcf0add27591fafa9c41dbe Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 10:46:29 +0100
Subject: [PATCH 096/416] add macros for easy 2d array support, builds on px4
 test build

---
 .../flow_position_estimator/flow_position_estimator_main.c | 3 ++-
 src/include/px4.h                                          | 2 +-
 src/{platforms => include}/px4_defines.h                   | 7 ++++++-
 3 files changed, 9 insertions(+), 3 deletions(-)
 rename src/{platforms => include}/px4_defines.h (94%)

diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 0b8c01f798..8cc9ed6862 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -67,6 +67,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <poll.h>
+#include <px4_defines.h>
 
 #include "flow_position_estimator_params.h"
 
@@ -337,7 +338,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 						{
 							float sum = 0.0f;
 							for(uint8_t j = 0; j < 3; j++)
-								sum = sum + speed[j] * att.R[i][j];
+								sum = sum + speed[j] * PX4_R(att.R, i, j);
 
 							global_speed[i] = sum;
 						}
diff --git a/src/include/px4.h b/src/include/px4.h
index 22d661b17c..45068a6f77 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -59,7 +59,7 @@
 
 #endif
 
-#include "../platforms/px4_defines.h"
+#include <px4_defines.h>
 #include "../platforms/px4_middleware.h"
 #include "../platforms/px4_nodehandle.h"
 #include "../platforms/px4_subscriber.h"
diff --git a/src/platforms/px4_defines.h b/src/include/px4_defines.h
similarity index 94%
rename from src/platforms/px4_defines.h
rename to src/include/px4_defines.h
index 84e5c8da09..2dd57940d1 100644
--- a/src/platforms/px4_defines.h
+++ b/src/include/px4_defines.h
@@ -51,7 +51,6 @@
 #define PX4_TOPIC_T(_name) _name
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
-
 #else
 /*
  * Building for NuttX
@@ -70,3 +69,9 @@
 #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
 #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
 #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
+
+/* wrapper for 2d matrices */
+#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y])
+
+/* wrapper for rotation matrices stored in arrays */
+#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)

From fbc497452517a748232a0b88b0a8f8f6ca8a9b92 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 10:50:31 +0100
Subject: [PATCH 097/416] remove rc_channels, vehicle_attitude topic header
 files

---
 src/modules/uORB/topics/rc_channels.h      | 96 ----------------------
 src/modules/uORB/topics/vehicle_attitude.h | 92 ---------------------
 2 files changed, 188 deletions(-)
 delete mode 100644 src/modules/uORB/topics/rc_channels.h
 delete mode 100755 src/modules/uORB/topics/vehicle_attitude.h

diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
deleted file mode 100644
index 16916cc4df..0000000000
--- a/src/modules/uORB/topics/rc_channels.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file rc_channels.h
- * Definition of the rc_channels uORB topic.
- *
- * @deprecated DO NOT USE FOR NEW CODE
- */
-
-#ifndef RC_CHANNELS_H_
-#define RC_CHANNELS_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * This defines the mapping of the RC functions.
- * The value assigned to the specific function corresponds to the entry of
- * the channel array channels[].
- */
-enum RC_CHANNELS_FUNCTION {
-	THROTTLE = 0,
-	ROLL,
-	PITCH,
-	YAW,
-	MODE,
-	RETURN,
-	POSCTL,
-	LOITER,
-	OFFBOARD,
-	ACRO,
-	FLAPS,
-	AUX_1,
-	AUX_2,
-	AUX_3,
-	AUX_4,
-	AUX_5
-};
-
-// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
-
-#define RC_CHANNELS_FUNCTION_MAX 18
-
-/**
- * @addtogroup topics
- * @{
- */
-struct rc_channels_s {
-	uint64_t timestamp;									/**< Timestamp in microseconds since boot time */
-	uint64_t timestamp_last_valid;						/**< Timestamp of last valid RC signal */
-	float channels[RC_CHANNELS_FUNCTION_MAX];			/**< Scaled to -1..1 (throttle: 0..1) */
-	uint8_t channel_count;								/**< Number of valid channels */
-	int8_t function[RC_CHANNELS_FUNCTION_MAX];			/**< Functions mapping */
-	uint8_t rssi;										/**< Receive signal strength index */
-	bool signal_lost;									/**< Control signal lost, should be checked together with topic timeout */
-}; /**< radio control channels. */
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(rc_channels);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
deleted file mode 100755
index 40328af146..0000000000
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 vehicle_attitude.h
- * Definition of the attitude uORB topic.
- */
-
-#ifndef VEHICLE_ATTITUDE_H_
-#define VEHICLE_ATTITUDE_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Attitude in NED body frame in SI units.
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- */
-struct vehicle_attitude_s {
-
-	uint64_t timestamp;	/**< in microseconds since system start          */
-
-	/* This is similar to the mavlink message ATTITUDE, but for onboard use */
-
-	/** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
-
-	float roll;		/**< Roll angle (rad, Tait-Bryan, NED)				*/
-	float pitch;		/**< Pitch angle (rad, Tait-Bryan, NED)				*/
-	float yaw;		/**< Yaw angle (rad, Tait-Bryan, NED)				*/
-	float rollspeed;	/**< Roll angular speed (rad/s, Tait-Bryan, NED)		*/
-	float pitchspeed;	/**< Pitch angular speed (rad/s, Tait-Bryan, NED)		*/
-	float yawspeed;		/**< Yaw angular speed (rad/s, Tait-Bryan, NED)			*/
-	float rollacc;		/**< Roll angular accelration (rad/s, Tait-Bryan, NED)		*/
-	float pitchacc;		/**< Pitch angular acceleration (rad/s, Tait-Bryan, NED)	*/
-	float yawacc;		/**< Yaw angular acceleration (rad/s, Tait-Bryan, NED)		*/
-	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
-	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
-	float q[4];		/**< Quaternion (NED)						*/
-	float g_comp[3];	/**< Compensated gravity vector					*/
-	bool R_valid;		/**< Rotation matrix valid					*/
-	bool q_valid;		/**< Quaternion valid						*/
-
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude);
-
-#endif

From eba62a75a445bf1f6302af7ea0cb24b458d65d25 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 12:17:06 +0100
Subject: [PATCH 098/416] add missing timestamp field

---
 msg/px4_msgs/rc_channels.msg | 1 +
 1 file changed, 1 insertion(+)

diff --git a/msg/px4_msgs/rc_channels.msg b/msg/px4_msgs/rc_channels.msg
index 19fd14c7cc..4e0e5b4944 100644
--- a/msg/px4_msgs/rc_channels.msg
+++ b/msg/px4_msgs/rc_channels.msg
@@ -15,6 +15,7 @@ uint8 RC_CHANNELS_FUNCTION_AUX_2=12
 uint8 RC_CHANNELS_FUNCTION_AUX_3=13
 uint8 RC_CHANNELS_FUNCTION_AUX_4=14
 uint8 RC_CHANNELS_FUNCTION_AUX_5=15
+uint64 timestamp						# Timestamp in microseconds since boot time
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels

From 141dda209222a7dd5afad6b844735fd9a44756a6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 12:17:34 +0100
Subject: [PATCH 099/416] fix uorb header template for constants

---
 msg/templates/msg.h.template | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index f60001b55d..5fce506b6b 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -82,7 +82,7 @@ for field in spec.parsed_fields():
 }@
 @# Constants
 @[for constant in spec.constants]@
-#define @(constant.name) = @(int(constant.val))
+#define @(constant.name) @(int(constant.val))
 @[end for]
 
 /**

From e28e8c11bba0779386fc16ee47deac4db39b51c0 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 12:19:24 +0100
Subject: [PATCH 100/416] make default apps compatible with autogenerated
 attitude and rc_channels message

---
 .../attitude_estimator_ekf_main.cpp           |  2 +-
 .../ekf_att_pos_estimator_main.cpp            |  3 +-
 .../fw_att_control/fw_att_control_main.cpp    |  7 +-
 .../fw_pos_control_l1_main.cpp                |  3 +-
 .../mc_pos_control/mc_pos_control_main.cpp    |  9 ++-
 .../position_estimator_inav_main.c            | 27 +++----
 src/modules/sensors/sensors.cpp               | 76 +++++++++----------
 7 files changed, 66 insertions(+), 61 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index e2dbc30dd5..8d537d6763 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -549,7 +549,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					R = R_decl * R_body;
 
 					/* copy rotation matrix */
-					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
+					memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index e7805daa91..db6773b8a0 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -83,6 +83,7 @@
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
+#include <px4_defines.h>
 
 #include "estimator_23states.h"
 
@@ -1384,7 +1385,7 @@ FixedwingEstimator::task_main()
 					math::Vector<3> euler = R.to_euler();
 
 					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-							_att.R[i][j] = R(i, j);
+							PX4_R(_att.R, i, j) = R(i, j);
 
 					_att.timestamp = _last_sensor_timestamp;
 					_att.q[0] = _ekf->states[0];
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index e770c11a27..83fe255711 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -75,6 +75,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
+#include <px4_defines.h>
 
 /**
  * Fixedwing attitude control app start / stop handling function
@@ -824,9 +825,9 @@ FixedwingAttitudeControl::task_main()
 				float speed_body_v = 0.0f;
 				float speed_body_w = 0.0f;
 				if(_att.R_valid) 	{
-					speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
-					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
-					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
+					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");
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 6017369aa5..f441c4a915 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -90,6 +90,7 @@
 #include <external_lgpl/tecs/tecs.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
+#include <px4_defines.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
 
@@ -704,7 +705,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
 
 		/* set rotation matrix */
 		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				_R_nb(i, j) = _att.R[i][j];
+				_R_nb(i, j) = PX4_R(_att.R, i, j);
 	}
 }
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 5918d6bc5f..6682a9c89c 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -73,6 +73,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <mavlink/mavlink_log.h>
+#include <px4_defines.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
@@ -1147,11 +1148,11 @@ MulticopterPositionControl::task_main()
 						/* thrust compensation for altitude only control mode */
 						float att_comp;
 
-						if (_att.R[2][2] > TILT_COS_MAX) {
-							att_comp = 1.0f / _att.R[2][2];
+						if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
+							att_comp = 1.0f / PX4_R(_att.R, 2, 2);
 
-						} else if (_att.R[2][2] > 0.0f) {
-							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+						} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
 							saturation_z = true;
 
 						} else {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 296919c042..2a601b6306 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -68,6 +68,7 @@
 #include <geo/geo.h>
 #include <systemlib/systemlib.h>
 #include <drivers/drv_hrt.h>
+#include <px4_defines.h>
 
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
@@ -282,13 +283,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	};
 	float w_gps_xy = 1.0f;
 	float w_gps_z = 1.0f;
-	
+
 	float corr_vision[3][2] = {
 		{ 0.0f, 0.0f },		// N (pos, vel)
 		{ 0.0f, 0.0f },		// E (pos, vel)
 		{ 0.0f, 0.0f },		// D (pos, vel)
 	};
-	
+
 	float corr_sonar = 0.0f;
 	float corr_sonar_filtered = 0.0f;
 
@@ -461,7 +462,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
 							}
 						}
 
@@ -494,7 +495,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if ((flow.ground_distance_m > 0.31f) &&
 					(flow.ground_distance_m < 4.0f) &&
-					(att.R[2][2] > 0.7f) &&
+					(PX4_R(att.R, 2, 2) > 0.7f) &&
 					(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
 
 					sonar_time = t;
@@ -531,15 +532,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				float flow_q = flow.quality / 255.0f;
 				float dist_bottom = - z_est[0] - surface_offset;
 
-				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
+				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
 					/* distance to surface */
-					float flow_dist = dist_bottom / att.R[2][2];
+					float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
 					/* check if flow if too large for accurate measurements */
 					/* calculate estimated velocity in body frame */
 					float body_v_est[2] = { 0.0f, 0.0f };
 
 					for (int i = 0; i < 2; i++) {
-						body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
+						body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
 					}
 
 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -562,7 +563,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* project measurements vector to NED basis, skip Z component */
 					for (int i = 0; i < 2; i++) {
 						for (int j = 0; j < 3; j++) {
-							flow_v[i] += att.R[i][j] * flow_m[j];
+							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
 						}
 					}
 
@@ -571,7 +572,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					corr_flow[1] = flow_v[1] - y_est[1];
 					/* adjust correction weight */
 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
-					w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
+					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
 
 					/* if flow is not accurate, reduce weight for it */
 					// TODO make this more fuzzy
@@ -646,13 +647,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						x_est[1] = vision.vx;
 						y_est[0] = vision.y;
 						y_est[1] = vision.vy;
-						/* only reset the z estimate if the z weight parameter is not zero */ 
+						/* only reset the z estimate if the z weight parameter is not zero */
 						if (params.w_z_vision_p > MIN_VALID_W)
 						{
 							z_est[0] = vision.z;
 							z_est[1] = vision.vz;
 						}
-						
+
 						vision_valid = true;
 						warnx("VISION estimate valid");
 						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
@@ -912,7 +913,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			float c = 0.0f;
 
 			for (int j = 0; j < 3; j++) {
-				c += att.R[j][i] * accel_bias_corr[j];
+				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
 			}
 
 			if (isfinite(c)) {
@@ -937,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			float c = 0.0f;
 
 			for (int j = 0; j < 3; j++) {
-				c += att.R[j][i] * accel_bias_corr[j];
+				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
 			}
 
 			if (isfinite(c)) {
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index cdcb428dd1..672dc52c3d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -182,13 +182,13 @@ private:
 	/**
 	 * Get and limit value for specified RC function. Returns NAN if not mapped.
 	 */
-	float		get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+	float		get_rc_value(uint8_t func, float min_value, float max_value);
 
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
-	switch_pos_t	get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
+	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
 
 	/**
 	 * Gather and publish RC input data.
@@ -771,25 +771,25 @@ Sensors::parameters_update()
 	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
 
 	/* update RC function mappings */
-	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
-	_rc.function[ROLL] = _parameters.rc_map_roll - 1;
-	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
-	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
 
-	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
-	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
-	_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
-	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
-	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
-	_rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
 
-	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
 
-	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
-	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
-	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
-	_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
-	_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
+	_rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
 
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
@@ -1492,7 +1492,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 }
 
 float
-Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
 {
 	if (_rc.function[func] >= 0) {
 		float value = _rc.channels[_rc.function[func]];
@@ -1513,7 +1513,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
 }
 
 switch_pos_t
-Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
+Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
 		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
@@ -1534,7 +1534,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
 }
 
 switch_pos_t
-Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 {
 	if (_rc.function[func] >= 0) {
 		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
@@ -1668,24 +1668,24 @@ Sensors::rc_poll()
 			manual.timestamp = rc_input.timestamp_last_signal;
 
 			/* limit controls */
-			manual.y = get_rc_value(ROLL, -1.0, 1.0);
-			manual.x = get_rc_value(PITCH, -1.0, 1.0);
-			manual.r = get_rc_value(YAW, -1.0, 1.0);
-			manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
-			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
-			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
-			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
-			manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
-			manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
-			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
+			manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
+			manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
+			manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
+			manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
+			manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
+			manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
+			manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
+			manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
+			manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
+			manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
 
 			/* mode switches */
-			manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
-			manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
-			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
-			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
-			manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
-			manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+			manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+			manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+			manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+			manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+			manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {

From 5bb03f1c2db3eb5620caf806b053f8194490969a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 15:21:58 +0100
Subject: [PATCH 101/416] subscriber example increase stack size

---
 src/examples/subscriber/module.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
index 44d3d03a92..481c36ea70 100644
--- a/src/examples/subscriber/module.mk
+++ b/src/examples/subscriber/module.mk
@@ -39,4 +39,4 @@ MODULE_COMMAND	= subscriber
 
 SRCS		= subscriber.cpp
 
-MODULE_STACKSIZE = 1200
+MODULE_STACKSIZE = 2400

From 8d3d8a3358c4c44d6de13a0c8e6a88ea98288bf2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 15:22:21 +0100
Subject: [PATCH 102/416] subscriber example clean up

---
 src/examples/subscriber/subscriber.cpp | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 3d54a1a2ff..a29f9ab934 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -36,7 +36,7 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
 }
 void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard(2): [%llu]", msg.timestamp_last_valid);
+	PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid);
 }
 
 class RCHandler {
@@ -91,14 +91,7 @@ PX4_MAIN_FUNCTION(subscriber) {
 	 */
 	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
 	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
-	//1
-	// PX4_SUBSCRIBE(n, rc_channels, callee.rc_channels_callback, , 1000);
-	//2
-	// PX4_SUBSCRIBE(n, rc_channels, rchandler.callback, &rchandler, 1000);
-	//3 for bind
 	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
-	// ros::NodeHandle n2;
-	// n2.subscribe("chatter", 1000, &RCHandler::callback, &rchandler);
 	PX4_INFO("subscribed");
 
 	/**

From f2fdd0d69bd485840c5ae491040322671e202fa2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 15:44:57 +0100
Subject: [PATCH 103/416] add mc_att_control to CMakeList

---
 CMakeLists.txt | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 636cc07e34..8b3ca17e31 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -136,6 +136,14 @@ target_link_libraries(subscriber
   px4
 )
 
+## mc_att_control
+add_executable(mc_att_control src/module/mc_att_control_main.cpp)
+add_dependencies(mc_att_control px4_generate_messages_cpp)
+target_link_libraries(mc_att_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
 #############
 ## Install ##
 #############

From 05a87a706a122b8a83becaaa94c70161fb69c82a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 2 Dec 2014 16:17:17 +0100
Subject: [PATCH 104/416] move px4_defines file

---
 .../flow_position_estimator/flow_position_estimator_main.c      | 2 +-
 src/include/px4.h                                               | 2 +-
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp        | 2 +-
 src/modules/fw_att_control/fw_att_control_main.cpp              | 2 +-
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp        | 2 +-
 src/modules/mc_pos_control/mc_pos_control_main.cpp              | 2 +-
 .../position_estimator_inav/position_estimator_inav_main.c      | 2 +-
 src/{include => platforms}/px4_defines.h                        | 0
 8 files changed, 7 insertions(+), 7 deletions(-)
 rename src/{include => platforms}/px4_defines.h (100%)

diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 8cc9ed6862..5a23932d3a 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -67,7 +67,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <poll.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #include "flow_position_estimator_params.h"
 
diff --git a/src/include/px4.h b/src/include/px4.h
index 45068a6f77..22d661b17c 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -59,7 +59,7 @@
 
 #endif
 
-#include <px4_defines.h>
+#include "../platforms/px4_defines.h"
 #include "../platforms/px4_middleware.h"
 #include "../platforms/px4_nodehandle.h"
 #include "../platforms/px4_subscriber.h"
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index db6773b8a0..c0b1bb4040 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -83,7 +83,7 @@
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #include "estimator_23states.h"
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 83fe255711..57c1e72f3c 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -75,7 +75,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 /**
  * Fixedwing attitude control app start / stop handling function
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index f441c4a915..e07bcc225f 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -90,7 +90,7 @@
 #include <external_lgpl/tecs/tecs.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 6682a9c89c..cea847603e 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -73,7 +73,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <mavlink/mavlink_log.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 2a601b6306..de6357d31b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -68,7 +68,7 @@
 #include <geo/geo.h>
 #include <systemlib/systemlib.h>
 #include <drivers/drv_hrt.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
diff --git a/src/include/px4_defines.h b/src/platforms/px4_defines.h
similarity index 100%
rename from src/include/px4_defines.h
rename to src/platforms/px4_defines.h

From 905913986a03cdb31d056b278cadc2d6f6421e38 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Dec 2014 07:43:08 +0100
Subject: [PATCH 105/416] update comments

---
 src/platforms/px4_nodehandle.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 4153517565..cf47fec592 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -55,7 +55,6 @@
 
 namespace px4
 {
-//XXX create abstract base class
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class NodeHandle :
 	private ros::NodeHandle
@@ -71,7 +70,7 @@ public:
 		//XXX empty lists
 	};
 
-	/* Constructor with callback to function */
+	/* Subscribe with callback to function */
 	template<typename M>
 	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
@@ -79,7 +78,8 @@ public:
 		_subs.push_back(sub);
 		return sub;
 	}
-	/* Constructor with callback to class method */
+
+	/* Subscribe with callback to class method */
 	template<typename M, typename T>
 	Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);

From 6a99b04fb74f6da3faea54c93d234a9b57d7bd0e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Dec 2014 12:31:37 +0100
Subject: [PATCH 106/416] add parameter wrapper macros for ros

---
 src/examples/subscriber/subscriber.cpp | 8 ++++++++
 src/platforms/px4_defines.h            | 2 ++
 2 files changed, 10 insertions(+)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index a29f9ab934..a8ecd4a7db 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -74,6 +74,14 @@ PX4_MAIN_FUNCTION(subscriber) {
 	 */
 	px4::NodeHandle n;
 
+	/* Define a parameter */
+	PX4_PARAM_INIT("SUB_INTERV", 100);
+
+	/* Read the parameter back for testing */
+	int32_t sub_interval;
+	PX4_PARAM_GET("SUB_INTERV", &sub_interval);
+	PX4_INFO("Param SUB_INTERV = %d", sub_interval);
+
 	/**
 	 * The subscribe() call is how you tell ROS that you want to receive messages
 	 * on a given topic.  This invokes a call to the ROS
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 2dd57940d1..2d9051aaea 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -51,6 +51,8 @@
 #define PX4_TOPIC_T(_name) _name
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default);
+#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt)
 #else
 /*
  * Building for NuttX

From 1c79f0cef1f21cff7935ddd7caf048fd96991eea Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Dec 2014 14:43:03 +0100
Subject: [PATCH 107/416] improve param wrapper for ros, prepare for px4

---
 src/examples/subscriber/subscriber.cpp | 12 ++++++++----
 src/include/px4.h                      |  1 +
 src/platforms/px4_defines.h            | 17 +++++++++++++++--
 src/platforms/px4_nodehandle.h         |  1 +
 4 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index a8ecd4a7db..e52d661a99 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -74,13 +74,17 @@ PX4_MAIN_FUNCTION(subscriber) {
 	 */
 	px4::NodeHandle n;
 
-	/* Define a parameter */
-	PX4_PARAM_INIT("SUB_INTERV", 100);
+	/* Define parameters */
+	px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100);
+	px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f);
 
 	/* Read the parameter back for testing */
 	int32_t sub_interval;
-	PX4_PARAM_GET("SUB_INTERV", &sub_interval);
+	float test_float;
+	PX4_PARAM_GET(p_sub_interv, &sub_interval);
 	PX4_INFO("Param SUB_INTERV = %d", sub_interval);
+	PX4_PARAM_GET(p_test_float, &test_float);
+	PX4_INFO("Param SUB_TESTF = %f", (double)test_float);
 
 	/**
 	 * The subscribe() call is how you tell ROS that you want to receive messages
@@ -97,7 +101,7 @@ PX4_MAIN_FUNCTION(subscriber) {
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval);
 	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
 	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
 	PX4_INFO("subscribed");
diff --git a/src/include/px4.h b/src/include/px4.h
index 22d661b17c..2d5d259648 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -56,6 +56,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
 #include <systemlib/err.h>
+#include <systemlib/param/param.h>
 
 #endif
 
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 2d9051aaea..a10df858a8 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -51,8 +51,18 @@
 #define PX4_TOPIC_T(_name) _name
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
-#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default);
-#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt)
+typedef const std::string px4_param_t;
+static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) {
+	ros::param::set(name, value);
+	return (px4_param_t)name;
+};
+static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) {
+	ros::param::set(name, value);
+	return (px4_param_t)name;
+};
+#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default)
+// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default)
+#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 #else
 /*
  * Building for NuttX
@@ -65,6 +75,9 @@
 #define PX4_TOPIC_T(_name) _name##_s
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+typedef param_t px4_param_t
+#define PX4_PARAM_INIT(_name, _default) param_find(_name)
+#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 #endif
 
 /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index cf47fec592..a2775d69ab 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -99,6 +99,7 @@ public:
 	void spin() { ros::spin(); }
 
 	void spinOnce() { ros::spinOnce(); }
+
 private:
 	static const uint32_t kQueueSizeDefault = 1000;
 	std::list<Subscriber*> _subs;

From 924350a5de1a609e470618ef78212bf7b0044c33 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Dec 2014 15:11:27 +0100
Subject: [PATCH 108/416] bring up param wrapper for px4, moved global include
 file

---
 src/examples/subscriber/module.mk           |  3 +-
 src/examples/subscriber/subscriber.cpp      |  2 +-
 src/examples/subscriber/subscriber_params.c | 55 +++++++++++++++++++
 src/include/px4.h                           | 22 +-------
 src/platforms/px4_defines.h                 |  4 +-
 src/platforms/px4_includes.h                | 61 +++++++++++++++++++++
 6 files changed, 123 insertions(+), 24 deletions(-)
 create mode 100644 src/examples/subscriber/subscriber_params.c
 create mode 100644 src/platforms/px4_includes.h

diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
index 481c36ea70..90b4d8ffc5 100644
--- a/src/examples/subscriber/module.mk
+++ b/src/examples/subscriber/module.mk
@@ -37,6 +37,7 @@
 
 MODULE_COMMAND	= subscriber
 
-SRCS		= subscriber.cpp
+SRCS		= subscriber.cpp \
+		  subscriber_params.c
 
 MODULE_STACKSIZE = 2400
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index e52d661a99..cc9b7a7942 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -84,7 +84,7 @@ PX4_MAIN_FUNCTION(subscriber) {
 	PX4_PARAM_GET(p_sub_interv, &sub_interval);
 	PX4_INFO("Param SUB_INTERV = %d", sub_interval);
 	PX4_PARAM_GET(p_test_float, &test_float);
-	PX4_INFO("Param SUB_TESTF = %f", (double)test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float);
 
 	/**
 	 * The subscribe() call is how you tell ROS that you want to receive messages
diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c
new file mode 100644
index 0000000000..e28bfbc363
--- /dev/null
+++ b/src/examples/subscriber/subscriber_params.c
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_params.c
+ * Parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Interval of one subscriber in the example in ms
+ *
+ * @group Subscriber Example
+ */
+PARAM_DEFINE_INT32(SUB_INTERV, 100);
+
+/**
+ * Float Demonstration Parameter in the Example
+ *
+ * @group Subscriber Example
+ */
+PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f);
diff --git a/src/include/px4.h b/src/include/px4.h
index 2d5d259648..ca702d63ce 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -39,27 +39,7 @@
 
 #pragma once
 
-#include <stdbool.h>
-
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
-/*
- * Building for running within the ROS environment
- */
-#include "ros/ros.h"
-#include "px4/rc_channels.h"
-
-#else
-/*
- * Building for NuttX
- */
-#include <nuttx/config.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/rc_channels.h>
-#include <systemlib/err.h>
-#include <systemlib/param/param.h>
-
-#endif
-
+#include "../platforms/px4_includes.h"
 #include "../platforms/px4_defines.h"
 #include "../platforms/px4_middleware.h"
 #include "../platforms/px4_nodehandle.h"
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index a10df858a8..a715075108 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -63,10 +63,12 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) {
 #define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default)
 // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default)
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
+
 #else
 /*
  * Building for NuttX
  */
+#include <platforms/px4_includes.h>
 #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
 #define PX4_WARN warnx
 #define PX4_WARN warnx
@@ -75,7 +77,7 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) {
 #define PX4_TOPIC_T(_name) _name##_s
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
-typedef param_t px4_param_t
+typedef param_t px4_param_t;
 #define PX4_PARAM_INIT(_name, _default) param_find(_name)
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 #endif
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
new file mode 100644
index 0000000000..a3b59b766e
--- /dev/null
+++ b/src/platforms/px4_includes.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_includes.h
+ *
+ * Includes headers depending on the build target
+ */
+
+#pragma once
+
+#include <stdbool.h>
+
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/*
+ * Building for running within the ROS environment
+ */
+#include "ros/ros.h"
+#include "px4/rc_channels.h"
+
+#else
+/*
+ * Building for NuttX
+ */
+#include <nuttx/config.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+
+#endif

From c2e2b3d52fa7bcfbbec110b5555d2c437657f118 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Dec 2014 17:04:15 +0100
Subject: [PATCH 109/416] make param wrapper macros compatible for px4 and ros,
 needs cleanup

---
 makefiles/setup.mk                          |  6 ++-
 src/examples/subscriber/subscriber.cpp      |  5 ++-
 src/examples/subscriber/subscriber_params.c |  7 ++--
 src/examples/subscriber/subscriber_params.h | 43 +++++++++++++++++++++
 src/platforms/px4_defines.h                 | 21 +++++++---
 5 files changed, 70 insertions(+), 12 deletions(-)
 create mode 100644 src/examples/subscriber/subscriber_params.h

diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 4bfa7a0876..435c240bfa 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -37,13 +37,14 @@
 # Some useful paths.
 #
 # Note that in general we always keep directory paths with the separator
-# at the end, and join paths without explicit separators. This reduces 
+# at the end, and join paths without explicit separators. This reduces
 # the number of duplicate slashes we have lying around in paths,
 # and is consistent with joining the results of $(dir) and $(notdir).
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
 export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
+export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export MAVLINK_SRC	 = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
@@ -61,7 +62,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
 			   $(PX4_INCLUDE_DIR) \
-			   $(PX4_LIB_DIR)
+			   $(PX4_LIB_DIR) \
+			   $(PX4_PLATFORMS_DIR)
 
 #
 # Tools
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index cc9b7a7942..68003b6dfc 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -26,6 +26,7 @@
  */
 
 #include <px4.h>
+#include "subscriber_params.h"
 
 using namespace px4;
 
@@ -75,8 +76,8 @@ PX4_MAIN_FUNCTION(subscriber) {
 	px4::NodeHandle n;
 
 	/* Define parameters */
-	px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100);
-	px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f);
+	px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV);
+	px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF);
 
 	/* Read the parameter back for testing */
 	int32_t sub_interval;
diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c
index e28bfbc363..0d36f5021d 100644
--- a/src/examples/subscriber/subscriber_params.c
+++ b/src/examples/subscriber/subscriber_params.c
@@ -38,18 +38,19 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <systemlib/param/param.h>
+#include <px4_defines.h>
+#include "subscriber_params.h"
 
 /**
  * Interval of one subscriber in the example in ms
  *
  * @group Subscriber Example
  */
-PARAM_DEFINE_INT32(SUB_INTERV, 100);
+PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV));
 
 /**
  * Float Demonstration Parameter in the Example
  *
  * @group Subscriber Example
  */
-PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f);
+PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF));
diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h
new file mode 100644
index 0000000000..3f3ff5bce8
--- /dev/null
+++ b/src/examples/subscriber/subscriber_params.h
@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_params.h
+ * Default parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define SUB_INTERV_DEFAULT_VALUE 100
+#define SUB_TESTF_DEFAULT_VALUE 3.14f
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index a715075108..6760f57ead 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -39,11 +39,15 @@
 
 #pragma once
 
+#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE
+
+
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /*
  * Building for running within the ROS environment
  */
 #define __EXPORT
+#include "ros/ros.h"
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
@@ -51,18 +55,21 @@
 #define PX4_TOPIC_T(_name) _name
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
-typedef const std::string px4_param_t;
-static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) {
+typedef const char* px4_param_t;
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) {
 	ros::param::set(name, value);
 	return (px4_param_t)name;
 };
-static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) {
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) {
 	ros::param::set(name, value);
 	return (px4_param_t)name;
 };
-#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default)
+// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default;
+// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default;
+#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default)
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
+#define PX4_PARAM_INT32_T int //XXX
 
 #else
 /*
@@ -78,8 +85,9 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) {
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
 typedef param_t px4_param_t;
-#define PX4_PARAM_INIT(_name, _default) param_find(_name)
+#define PX4_PARAM_INIT(_name) param_find(#_name)
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
+#define PX4_PARAM_INT32_T int32_t
 #endif
 
 /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
@@ -92,3 +100,6 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
+
+// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value;
+// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value;

From 83d97fd1c27728145e4869f957afcc7cae57ff8a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 09:53:37 +0100
Subject: [PATCH 110/416] better support for param default values on ros and
 px4

---
 src/examples/subscriber/subscriber_params.c | 4 ++--
 src/examples/subscriber/subscriber_params.h | 4 ++--
 src/platforms/px4_defines.h                 | 4 +++-
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c
index 0d36f5021d..a43817b5b9 100644
--- a/src/examples/subscriber/subscriber_params.c
+++ b/src/examples/subscriber/subscriber_params.c
@@ -46,11 +46,11 @@
  *
  * @group Subscriber Example
  */
-PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV));
+PX4_PARAM_DEFINE_INT32(SUB_INTERV);
 
 /**
  * Float Demonstration Parameter in the Example
  *
  * @group Subscriber Example
  */
-PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF));
+PX4_PARAM_DEFINE_FLOAT(SUB_TESTF);
diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h
index 3f3ff5bce8..f6f1d6ba0c 100644
--- a/src/examples/subscriber/subscriber_params.h
+++ b/src/examples/subscriber/subscriber_params.h
@@ -39,5 +39,5 @@
  */
 #pragma once
 
-#define SUB_INTERV_DEFAULT_VALUE 100
-#define SUB_TESTF_DEFAULT_VALUE 3.14f
+#define PARAM_SUB_INTERV_DEFAULT 100
+#define PARAM_SUB_TESTF_DEFAULT 3.14f
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 6760f57ead..09338acabf 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -39,7 +39,9 @@
 
 #pragma once
 
-#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE
+#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
+#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))

From 8e8f84bde0d2e15734d931ea38a7b294a06d7314 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 10:39:24 +0100
Subject: [PATCH 111/416] uorb topic header generator: only create new files if
 the file content really changed

---
 Makefile                                |   5 +-
 Tools/px_generate_uorb_topic_headers.py | 121 +++++++++++++++++-------
 2 files changed, 90 insertions(+), 36 deletions(-)

diff --git a/Makefile b/Makefile
index 7df8004a40..bdbc18be54 100644
--- a/Makefile
+++ b/Makefile
@@ -227,12 +227,15 @@ updatesubmodules:
 MSG_DIR = $(PX4_BASE)msg/px4_msgs
 MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
+TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
 
 .PHONY: generateuorbtopicheaders
 generateuorbtopicheaders:
 	@$(ECHO) "Generating uORB topic headers"
 	$(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \
-		-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR))
+		-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
+# clean up temporary files
+	$(Q) (rm -r $(TOPICS_TEMPORARY_DIR))
 
 #
 # Testing targets
diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
index a738dcb7ef..2ddbd69847 100755
--- a/Tools/px_generate_uorb_topic_headers.py
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -39,6 +39,8 @@ message files
 """
 from __future__ import print_function
 import os
+import shutil
+import filecmp
 import argparse
 import genmsg.template_tools
 
@@ -55,44 +57,93 @@ package = 'px4'
 
 
 def convert_file(filename, outputdir, templatedir, includepath):
-    """
-    Converts a single .msg file to a uorb header
-    """
-    print("Generating uORB headers from {0}".format(filename))
-    genmsg.template_tools.generate_from_file(filename,
-                                             package,
-                                             outputdir,
-                                             templatedir,
-                                             includepath,
-                                             msg_template_map,
-                                             srv_template_map)
+        """
+        Converts a single .msg file to a uorb header
+        """
+        print("Generating uORB headers from {0}".format(filename))
+        genmsg.template_tools.generate_from_file(filename,
+                                                 package,
+                                                 outputdir,
+                                                 templatedir,
+                                                 includepath,
+                                                 msg_template_map,
+                                                 srv_template_map)
 
 
 def convert_dir(inputdir, outputdir, templatedir):
-    """
-    Converts all .msg files in inputdir to uORB header files
-    """
-    includepath = incl_default + [':'.join([package, inputdir])]
-    for f in os.listdir(inputdir):
-        fn = os.path.join(inputdir, f)
-        if os.path.isfile(fn):
-            convert_file(fn, outputdir, templatedir, includepath)
+        """
+        Converts all .msg files in inputdir to uORB header files
+        """
+        includepath = incl_default + [':'.join([package, inputdir])]
+        for f in os.listdir(inputdir):
+                fn = os.path.join(inputdir, f)
+                if os.path.isfile(fn):
+                        convert_file(
+                            fn,
+                            outputdir,
+                            templatedir,
+                            includepath)
+
+
+def copy_changed(inputdir, outputdir):
+        """
+        Copies files from inputdir to outputdir if they don't exist in
+        ouputdir or if their content changed
+        """
+        for f in os.listdir(inputdir):
+                fni = os.path.join(inputdir, f)
+                if os.path.isfile(fni):
+                        # Check if f exists in outpoutdir, copy the file if not
+                        fno = os.path.join(outputdir, f)
+                        if not os.path.isfile(fno):
+                                shutil.copy(fni, fno)
+                                print("{0}: new header file".format(f))
+                                continue
+                        # The file exists in inputdir and outputdir
+                        # only copy if contents do not match
+                        if not filecmp.cmp(fni, fno):
+                                shutil.copy(fni, fno)
+                                print("{0}: updated".format(f))
+                                continue
+
+                        print("{0}: unchanged".format(f))
+
+def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
+        """
+        Converts all .msg files in inputdir to uORB header files
+        Unchanged existing files are not overwritten.
+        """
+        # Create new headers in temporary output directory
+        convert_dir(inputdir, temporarydir, templatedir)
+
+        # Copy changed headers from temporary dir to output dir
+        copy_changed(temporarydir, outputdir)
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser(
-        description='Convert msg files to uorb headers')
-    parser.add_argument('-d', dest='dir', help='directory with msg files')
-    parser.add_argument('-f', dest='file',
-                        help="files to convert (use only without -d)",
-                        nargs="+")
-    parser.add_argument('-e', dest='templatedir',
-                        help='directory with template files',)
-    parser.add_argument('-o', dest='outputdir',
-                        help='output directory for header files')
-    args = parser.parse_args()
+        parser = argparse.ArgumentParser(
+            description='Convert msg files to uorb headers')
+        parser.add_argument('-d', dest='dir', help='directory with msg files')
+        parser.add_argument('-f', dest='file',
+                            help="files to convert (use only without -d)",
+                            nargs="+")
+        parser.add_argument('-e', dest='templatedir',
+                            help='directory with template files',)
+        parser.add_argument('-o', dest='outputdir',
+                            help='output directory for header files')
+        parser.add_argument('-t', dest='temporarydir',
+                            help='temporary directory')
+        args = parser.parse_args()
 
-    if args.file is not None:
-        for f in args.file:
-            convert_file(f, args.outputdir, args.templatedir, incl_default)
-    elif args.dir is not None:
-            convert_dir(args.dir, args.outputdir, args.templatedir)
+        if args.file is not None:
+                for f in args.file:
+                        convert_file(
+                            f,
+                            args.outputdir,
+                            args.templatedir,
+                            incl_default)
+        elif args.dir is not None:
+                convert_dir_save(
+                    args.dir,
+                    args.outputdir,
+                    args.templatedir,
+                    args.temporarydir)

From befe4c399e1f0f2864f91ddde4db585dabf3db99 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 11:50:59 +0100
Subject: [PATCH 112/416] comments for px4 defines

---
 src/platforms/px4_defines.h | 53 +++++++++++++++++++++++++++++--------
 1 file changed, 42 insertions(+), 11 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 09338acabf..5858a69d86 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -38,8 +38,10 @@
  */
 
 #pragma once
-
+/* Get the name of the default value fiven the param name */
 #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
+
+/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */
 #define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 
@@ -50,14 +52,28 @@
  */
 #define __EXPORT
 #include "ros/ros.h"
+/* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
+
+/* Print/output wrappers */
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
+
+/* Topic Handle */
 #define PX4_TOPIC(_name) #_name
+
+/* Topic type */
 #define PX4_TOPIC_T(_name) _name
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+
+/* Parameter handle datatype */
 typedef const char* px4_param_t;
+
+/* Helper fucntions to set ROS params, only int and float supported */
 static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) {
 	ros::param::set(name, value);
 	return (px4_param_t)name;
@@ -66,35 +82,53 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) {
 	ros::param::set(name, value);
 	return (px4_param_t)name;
 };
-// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default;
-// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default;
+
+/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */
 #define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
-// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default)
+
+/* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
-#define PX4_PARAM_INT32_T int //XXX
 
 #else
 /*
  * Building for NuttX
  */
 #include <platforms/px4_includes.h>
+/* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
-#define PX4_WARN warnx
+
+/* Print/output wrappers */
 #define PX4_WARN warnx
 #define PX4_INFO warnx
+
+/* Topic Handle */
 #define PX4_TOPIC(_name) ORB_ID(_name)
+
+/* Topic type */
 #define PX4_TOPIC_T(_name) _name##_s
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+
+/* Parameter handle datatype */
 typedef param_t px4_param_t;
+
+/* Initialize a param, get param handle */
 #define PX4_PARAM_INIT(_name) param_find(#_name)
+
+/* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
-#define PX4_PARAM_INT32_T int32_t
 #endif
 
-/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
+/* Shortcut for subscribing to topics
+ * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback
+ */
 #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
 #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
+
+/* shortcut for advertising topics */
 #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
 
 /* wrapper for 2d matrices */
@@ -102,6 +136,3 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
-
-// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value;
-// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value;

From 0f094d35d5dd183e44148eb1acbd7b7d76fde669 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 12:22:21 +0100
Subject: [PATCH 113/416] nodehandle: add documentation comments

---
 src/platforms/px4_nodehandle.h | 54 ++++++++++++++++++++++++++++------
 1 file changed, 45 insertions(+), 9 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index a2775d69ab..1762e16568 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -70,7 +70,11 @@ public:
 		//XXX empty lists
 	};
 
-	/* Subscribe with callback to function */
+	/**
+	 * Subscribe with callback to function
+	 * @param topic		Name of the topic
+	 * @param fb		Callback, executed on receiving a new message
+	 */
 	template<typename M>
 	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
@@ -79,7 +83,11 @@ public:
 		return sub;
 	}
 
-	/* Subscribe with callback to class method */
+	/**
+	 * Subscribe with callback to class method
+	 * @param topic		Name of the topic
+	 * @param fb		Callback, executed on receiving a new message
+	 */
 	template<typename M, typename T>
 	Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
@@ -88,6 +96,10 @@ public:
 		return sub;
 	}
 
+	/**
+	 * Advertise topic
+	 * @param topic		Name of the topic
+	 */
 	template<typename M>
 	Publisher * advertise(const char *topic) {
 		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
@@ -96,14 +108,21 @@ public:
 		return pub;
 	}
 
-	void spin() { ros::spin(); }
-
+	/**
+	 * Calls all callback waiting to be called
+	 */
 	void spinOnce() { ros::spinOnce(); }
 
+	/**
+	 * Keeps calling callbacks for incomming messages, returns when module is terminated
+	 */
+	void spin() { ros::spin(); }
+
+
 private:
-	static const uint32_t kQueueSizeDefault = 1000;
-	std::list<Subscriber*> _subs;
-	std::list<Publisher*> _pubs;
+	static const uint32_t kQueueSizeDefault = 1000;		/**< Size of queue for ROS */
+	std::list<Subscriber*> _subs;				/**< Subcriptions of node */
+	std::list<Publisher*> _pubs;				/**< Publications of node */
 };
 #else
 class __EXPORT NodeHandle
@@ -117,6 +136,13 @@ public:
 
 	~NodeHandle() {};
 
+	/**
+	 * Subscribe with callback to function
+	 * @param meta		Describes the topic which nodehande should subscribe to
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 */
+
 	template<typename M>
 	Subscriber * subscribe(const struct orb_metadata *meta,
 			std::function<void(const M&)> callback,
@@ -130,6 +156,10 @@ public:
 		return (Subscriber*)sub_px4;
 	}
 
+	/**
+	 * Advertise topic
+	 * @param meta		Describes the topic which is advertised
+	 */
 	template<typename M>
 	Publisher * advertise(const struct orb_metadata *meta) {
 		//XXX
@@ -137,6 +167,9 @@ public:
 		return pub;
 	}
 
+	/**
+	 * Calls all callback waiting to be called
+	 */
 	void spinOnce() {
 		/* Loop through subscriptions, call callback for updated subscriptions */
 		uORB::SubscriptionNode *sub = _subs.getHead();
@@ -153,6 +186,9 @@ public:
 		}
 	}
 
+	/**
+	 * Keeps calling callbacks for incomming messages, returns when module is terminated
+	 */
 	void spin() {
 		while (ok()) {
 			const int timeout_ms = 100;
@@ -176,8 +212,8 @@ public:
 	}
 private:
 	static const uint16_t kMaxSubscriptions = 100;
-	List<uORB::SubscriptionNode*> _subs;
-	List<uORB::PublicationNode*> _pubs;
+	List<uORB::SubscriptionNode*> _subs;		/**< Subcriptions of node */
+	List<uORB::PublicationNode*> _pubs;		/**< Publications of node */
 	uORB::SubscriptionNode* _sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };

From e0bb713bb03c0aa79e69496c787c012a8e1b78d1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 13:06:38 +0100
Subject: [PATCH 114/416] more documentation comments

---
 src/platforms/px4_middleware.h | 13 +++++++++++++
 src/platforms/px4_publisher.h  | 28 ++++++++++++++++++++++++----
 src/platforms/px4_subscriber.h | 32 +++++++++++++++++++++++++++++---
 3 files changed, 66 insertions(+), 7 deletions(-)

diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index dece729070..c1465b4b1b 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -50,9 +50,15 @@ __EXPORT void init(int argc, char *argv[], const char *process_name);
 __EXPORT uint64_t get_time_micros();
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/**
+ * Returns true if the app/task should continue to run
+ */
 bool ok() { return ros::ok(); }
 #else
 extern bool task_should_exit;
+/**
+ * Returns true if the app/task should continue to run
+ */
 bool ok() { return !task_should_exit; }
 #endif
 
@@ -60,8 +66,15 @@ class Rate
 {
 
 public:
+	/**
+	 * Construct the Rate object and set rate
+	 * @param rate_hz rate from which sleep time is calculated in Hz
+	 */
 	explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
 
+	/**
+	 * Sleep for 1/rate_hz s
+	 */
 	void sleep() { usleep(sleep_interval); }
 
 private:
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 9ce211d250..b67421066a 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -52,33 +52,53 @@ namespace px4
 class Publisher
 {
 public:
+	/**
+	 * Construct Publisher by providing a ros::Publisher
+	 * @param ros_pub the ros publisher which will be used to perform the publications
+	 */
 	Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
 	{}
+
 	~Publisher() {};
+
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
 	template<typename M>
 	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
 private:
-	ros::Publisher _ros_pub;
+	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
 class Publisher :
 	public uORB::PublicationNode
 {
 public:
+	/**
+	 * Construct Publisher by providing orb meta data
+	 * @param meta	    orb metadata for the topic which is used
+	 * @param list	    publisher is added to this list
+	 */
 	Publisher(const struct orb_metadata *meta,
 			List<uORB::PublicationNode *> * list) :
 		uORB::PublicationNode(meta, list)
 	{}
+
 	~Publisher() {};
+
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
 	template<typename M>
 	int publish(const M &msg) {
 		uORB::PublicationBase::update((void*)&msg);
 		return 0;
 	}
 
-	void update() {
-	//XXX list traversal callback, needed?
-	} ;
+	/**
+	 * Empty callback for list traversal
+	 */
+	void update() {} ;
 };
 #endif
 }
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 6e0ef8aed6..fdd0367d59 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -54,15 +54,24 @@ namespace px4
 class Subscriber
 {
 public:
+	/**
+	 * Construct Subscriber by providing a ros::Subscriber
+	 * @param ros_sub the ros subscriber which will be used to perform the publications
+	 */
 	Subscriber(ros::Subscriber ros_sub) :
 		_ros_sub(ros_sub)
 	{}
+
 	~Subscriber() {};
 private:
-	ros::Subscriber _ros_sub;
+	ros::Subscriber _ros_sub;	/**< Handle to ros subscriber */
 };
+
 #else
-// typedef std::function<void(int)> CallbackFunction;
+
+/**
+ * Subscriber class which is used by nodehandle when building for NuttX
+ */
 class Subscriber
 {
 public:
@@ -71,12 +80,22 @@ public:
 private:
 };
 
+/**
+ * Subscriber class that is templated with the uorb subscription message type
+ */
 template<typename M>
 class SubscriberPX4 :
 	public Subscriber,
 	public uORB::Subscription<M>
 {
 public:
+	/**
+	 * Construct SubscriberPX4 by providing orb meta data
+	 * @param meta	    orb metadata for the topic which is used
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 * @param list	    subscriber is added to this list
+	 */
 	SubscriberPX4(const struct orb_metadata *meta,
 			unsigned interval,
 			std::function<void(const M&)> callback,
@@ -86,8 +105,14 @@ public:
 		_callback(callback)
 		//XXX store callback
 	{}
+
 	~SubscriberPX4() {};
 
+	/**
+	 * Update Subscription
+	 * Invoked by the list traversal in NodeHandle::spinOnce
+	 * If new data is available the callback is called
+	 */
 	void update() {
 		if (!uORB::Subscription<M>::updated()) {
 			/* Topic not updated, do not call callback */
@@ -102,7 +127,8 @@ public:
 
 	};
 private:
-	std::function<void(const M&)> _callback;
+	std::function<void(const M&)> _callback;	/**< Callback handle,
+							  called when new data is available */
 
 };
 #endif

From f4a326c2bf7af6eac86983cd65e66ff76e623e22 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 13:20:12 +0100
Subject: [PATCH 115/416] platform headers: fix code style

---
 src/platforms/px4_defines.h    |  8 +++---
 src/platforms/px4_nodehandle.h | 49 +++++++++++++++++++++-------------
 src/platforms/px4_publisher.h  |  7 ++---
 src/platforms/px4_subscriber.h | 11 ++++----
 4 files changed, 45 insertions(+), 30 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 5858a69d86..af7188f32c 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -71,14 +71,16 @@
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 
 /* Parameter handle datatype */
-typedef const char* px4_param_t;
+typedef const char *px4_param_t;
 
 /* Helper fucntions to set ROS params, only int and float supported */
-static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) {
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
+{
 	ros::param::set(name, value);
 	return (px4_param_t)name;
 };
-static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) {
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
+{
 	ros::param::set(name, value);
 	return (px4_param_t)name;
 };
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 1762e16568..5b7247b203 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -66,7 +66,8 @@ public:
 		_pubs()
 	{}
 
-	~NodeHandle() {
+	~NodeHandle()
+	{
 		//XXX empty lists
 	};
 
@@ -76,9 +77,10 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M>
-	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
+	Subscriber *subscribe(const char *topic, void(*fp)(M))
+	{
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
-		Subscriber * sub = new Subscriber(ros_sub);
+		Subscriber *sub = new Subscriber(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
@@ -89,9 +91,10 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M, typename T>
-	Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
+	Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj)
+	{
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
-		Subscriber * sub = new Subscriber(ros_sub);
+		Subscriber *sub = new Subscriber(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
@@ -101,7 +104,8 @@ public:
 	 * @param topic		Name of the topic
 	 */
 	template<typename M>
-	Publisher * advertise(const char *topic) {
+	Publisher *advertise(const char *topic)
+	{
 		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
 		Publisher *pub =  new Publisher(ros_pub);
 		_pubs.push_back(pub);
@@ -121,8 +125,8 @@ public:
 
 private:
 	static const uint32_t kQueueSizeDefault = 1000;		/**< Size of queue for ROS */
-	std::list<Subscriber*> _subs;				/**< Subcriptions of node */
-	std::list<Publisher*> _pubs;				/**< Publications of node */
+	std::list<Subscriber *> _subs;				/**< Subcriptions of node */
+	std::list<Publisher *> _pubs;				/**< Publications of node */
 };
 #else
 class __EXPORT NodeHandle
@@ -144,16 +148,18 @@ public:
 	 */
 
 	template<typename M>
-	Subscriber * subscribe(const struct orb_metadata *meta,
-			std::function<void(const M&)> callback,
-			unsigned interval) {
+	Subscriber *subscribe(const struct orb_metadata *meta,
+			      std::function<void(const M &)> callback,
+			      unsigned interval)
+	{
 		SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
 		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
 			_sub_min_interval = sub_px4;
 		}
-		return (Subscriber*)sub_px4;
+
+		return (Subscriber *)sub_px4;
 	}
 
 	/**
@@ -161,16 +167,18 @@ public:
 	 * @param meta		Describes the topic which is advertised
 	 */
 	template<typename M>
-	Publisher * advertise(const struct orb_metadata *meta) {
+	Publisher *advertise(const struct orb_metadata *meta)
+	{
 		//XXX
-		Publisher * pub = new Publisher(meta, &_pubs);
+		Publisher *pub = new Publisher(meta, &_pubs);
 		return pub;
 	}
 
 	/**
 	 * Calls all callback waiting to be called
 	 */
-	void spinOnce() {
+	void spinOnce()
+	{
 		/* Loop through subscriptions, call callback for updated subscriptions */
 		uORB::SubscriptionNode *sub = _subs.getHead();
 		int count = 0;
@@ -189,9 +197,11 @@ public:
 	/**
 	 * Keeps calling callbacks for incomming messages, returns when module is terminated
 	 */
-	void spin() {
+	void spin()
+	{
 		while (ok()) {
 			const int timeout_ms = 100;
+
 			/* Only continue in the loop if the nodehandle has subscriptions */
 			if (_sub_min_interval == nullptr) {
 				usleep(timeout_ms * 1000);
@@ -202,6 +212,7 @@ public:
 			struct pollfd pfd;
 			pfd.fd = _sub_min_interval->getHandle();
 			pfd.events = POLLIN;
+
 			if (poll(&pfd, 1, timeout_ms) <= 0) {
 				/* timed out */
 				continue;
@@ -212,9 +223,9 @@ public:
 	}
 private:
 	static const uint16_t kMaxSubscriptions = 100;
-	List<uORB::SubscriptionNode*> _subs;		/**< Subcriptions of node */
-	List<uORB::PublicationNode*> _pubs;		/**< Publications of node */
-	uORB::SubscriptionNode* _sub_min_interval;	/**< Points to the sub wtih the smallest interval
+	List<uORB::SubscriptionNode *> _subs;		/**< Subcriptions of node */
+	List<uORB::PublicationNode *> _pubs;		/**< Publications of node */
+	uORB::SubscriptionNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };
 #endif
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index b67421066a..cb15eeb58f 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -80,7 +80,7 @@ public:
 	 * @param list	    publisher is added to this list
 	 */
 	Publisher(const struct orb_metadata *meta,
-			List<uORB::PublicationNode *> * list) :
+		  List<uORB::PublicationNode *> *list) :
 		uORB::PublicationNode(meta, list)
 	{}
 
@@ -90,8 +90,9 @@ public:
 	 * @param msg	    the message which is published to the topic
 	 */
 	template<typename M>
-	int publish(const M &msg) {
-		uORB::PublicationBase::update((void*)&msg);
+	int publish(const M &msg)
+	{
+		uORB::PublicationBase::update((void *)&msg);
 		return 0;
 	}
 
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index fdd0367d59..e995c6e49d 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -97,9 +97,9 @@ public:
 	 * @param list	    subscriber is added to this list
 	 */
 	SubscriberPX4(const struct orb_metadata *meta,
-			unsigned interval,
-			std::function<void(const M&)> callback,
-			List<uORB::SubscriptionNode *> * list) :
+		      unsigned interval,
+		      std::function<void(const M &)> callback,
+		      List<uORB::SubscriptionNode *> *list) :
 		Subscriber(),
 		uORB::Subscription<M>(meta, interval, list),
 		_callback(callback)
@@ -113,7 +113,8 @@ public:
 	 * Invoked by the list traversal in NodeHandle::spinOnce
 	 * If new data is available the callback is called
 	 */
-	void update() {
+	void update()
+	{
 		if (!uORB::Subscription<M>::updated()) {
 			/* Topic not updated, do not call callback */
 			return;
@@ -127,7 +128,7 @@ public:
 
 	};
 private:
-	std::function<void(const M&)> _callback;	/**< Callback handle,
+	std::function<void(const M &)> _callback;	/**< Callback handle,
 							  called when new data is available */
 
 };

From 6f9cbd97510dce4d0bfe715fa77af3b99d282659 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Dec 2014 16:40:46 +0100
Subject: [PATCH 116/416] add genmsg and gencpp python modules, ros not
 required anymore for message generation

---
 .gitmodules  | 6 ++++++
 Makefile     | 7 +++++--
 Tools/gencpp | 1 +
 Tools/genmsg | 1 +
 4 files changed, 13 insertions(+), 2 deletions(-)
 create mode 160000 Tools/gencpp
 create mode 160000 Tools/genmsg

diff --git a/.gitmodules b/.gitmodules
index 4b84afac2a..4996b274bd 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -7,3 +7,9 @@
 [submodule "uavcan"]
 	path = uavcan
 	url = git://github.com/pavel-kirienko/uavcan.git
+[submodule "Tools/genmsg"]
+	path = Tools/genmsg
+	url = https://github.com/ros/genmsg.git
+[submodule "Tools/gencpp"]
+	path = Tools/gencpp
+	url = https://github.com/ros/gencpp.git
diff --git a/Makefile b/Makefile
index bdbc18be54..f2e467e5aa 100644
--- a/Makefile
+++ b/Makefile
@@ -228,12 +228,15 @@ MSG_DIR = $(PX4_BASE)msg/px4_msgs
 MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
 TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
+GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src
+GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src
 
 .PHONY: generateuorbtopicheaders
 generateuorbtopicheaders:
 	@$(ECHO) "Generating uORB topic headers"
-	$(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \
-		-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
+	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
+		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
+		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
 # clean up temporary files
 	$(Q) (rm -r $(TOPICS_TEMPORARY_DIR))
 
diff --git a/Tools/gencpp b/Tools/gencpp
new file mode 160000
index 0000000000..26a86f04bc
--- /dev/null
+++ b/Tools/gencpp
@@ -0,0 +1 @@
+Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a
diff --git a/Tools/genmsg b/Tools/genmsg
new file mode 160000
index 0000000000..72f0383f0e
--- /dev/null
+++ b/Tools/genmsg
@@ -0,0 +1 @@
+Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454

From 4c950eb76b0d63be7936dfcd68eb6eed266b91ad Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 08:57:24 +0100
Subject: [PATCH 117/416] mc att control: make the main app use the base class,
 move euroc functions into own class

---
 .../mc_att_control/mc_att_control_base.cpp    | 120 +------
 .../mc_att_control/mc_att_control_base.h      |  17 +-
 .../mc_att_control/mc_att_control_main.cpp    | 300 ++----------------
 .../mc_att_control/mc_att_control_sim.cpp     | 107 +++++++
 .../mc_att_control/mc_att_control_sim.h       |  95 ++++++
 src/modules/mc_att_control/module.mk          |   3 +-
 6 files changed, 241 insertions(+), 401 deletions(-)
 create mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp
 create mode 100644 src/modules/mc_att_control/mc_att_control_sim.h

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index baf2bfe65c..d21deb7157 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -31,7 +31,7 @@
 
 /**
  * @file mc_att_control_base.h
- * 
+ *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -47,13 +47,12 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+	_task_should_exit(false),
+	_control_task(-1),
 
-		_task_should_exit(false), _control_task(-1),
+	_actuators_0_circuit_breaker_enabled(false),
 
-		_actuators_0_circuit_breaker_enabled(false),
-
-		/* performance counters */
-		_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+	_publish_att_sp(false)
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
@@ -82,38 +81,14 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_att_control.zero();
 
 	_I.identity();
-
-	// setup standard gains
-	_params.att_p(0)      = 5.0;
-	_params.rate_p(0)     = 0.05;
-	_params.rate_i(0)     = 0.0;
-	_params.rate_d(0)     = 0.003;
-	/* pitch gains */
-	_params.att_p(1)      = 5.0;
-	_params.rate_p(1)     = 0.05;
-	_params.rate_i(1)     = 0.0;
-	_params.rate_d(1)     = 0.003;
-	/* yaw gains */
-	_params.att_p(2)      = 2.8;
-	_params.rate_p(2)     = 0.2;
-	_params.rate_i(2)     = 0.1;
-	_params.rate_d(2)     = 0.0;
-	_params.yaw_rate_max  = 0.5;
-	_params.yaw_ff        = 0.5;
-	_params.man_roll_max  = 0.6;
-	_params.man_pitch_max = 0.6;
-	_params.man_yaw_max   = 0.6;
 }
 
 MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
 }
 
-void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() {
-}
-
 void MulticopterAttitudeControlBase::control_attitude(float dt) {
 	float yaw_sp_move_rate = 0.0f;
-	bool publish_att_sp = false;
+	_publish_att_sp = false;
 
 	if (_v_control_mode.flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
@@ -127,7 +102,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 		if (!_v_control_mode.flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
 			_v_att_sp.thrust = _manual_control_sp.z;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 		if (!_armed.armed) {
@@ -156,7 +131,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
 			}
 			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 		/* reset yaw setpint to current position if needed */
@@ -164,7 +139,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			_reset_yaw_sp = false;
 			_v_att_sp.yaw_body = _v_att.yaw;
 			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 		if (!_v_control_mode.flag_control_velocity_enabled) {
@@ -173,7 +148,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			_v_att_sp.pitch_body = -_manual_control_sp.x
 					* _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 	} else {
@@ -204,20 +179,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 		_v_att_sp.R_valid = true;
 	}
 
-//	/* publish the attitude setpoint if needed */
-//	if (publish_att_sp) {
-//		_v_att_sp.timestamp = hrt_absolute_time();
-//
-//		if (_att_sp_pub > 0) {
-//			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-//					&_v_att_sp);
-//
-//		} else {
-//			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-//					&_v_att_sp);
-//		}
-//	}
-
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
 	R.set(_v_att.R);
@@ -340,64 +301,3 @@ void MulticopterAttitudeControlBase::set_actuator_controls() {
 	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 }
-
-void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
-	math::Quaternion quat;
-	quat(0) = (float)attitude.w();
-	quat(1) = (float)attitude.x();
-	quat(2) = (float)attitude.y();
-	quat(3) = (float)attitude.z();
-
-	_v_att.q[0] = quat(0);
-	_v_att.q[1] = quat(1);
-	_v_att.q[2] = quat(2);
-	_v_att.q[3] = quat(3);
-
-	math::Matrix<3,3> Rot = quat.to_dcm();
-	_v_att.R[0][0] = Rot(0,0);
-	_v_att.R[1][0] = Rot(1,0);
-	_v_att.R[2][0] = Rot(2,0);
-	_v_att.R[0][1] = Rot(0,1);
-	_v_att.R[1][1] = Rot(1,1);
-	_v_att.R[2][1] = Rot(2,1);
-	_v_att.R[0][2] = Rot(0,2);
-	_v_att.R[1][2] = Rot(1,2);
-	_v_att.R[2][2] = Rot(2,2);
-
-	_v_att.R_valid = true;
-}
-
-void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
-	// check if this is consistent !!!
-	_v_att.rollspeed  = angular_rate(0);
-	_v_att.pitchspeed = angular_rate(1);
-	_v_att.yawspeed   = angular_rate(2);
-}
-
-void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
-	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
-	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
-	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
-
-	// setup rotation matrix
-	math::Matrix<3,3> Rot_sp;
-	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
-	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
-	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
-	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
-	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
-	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
-	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
-	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
-	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
-	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
-}
-
-void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
-	motor_inputs(0) = _actuators.control[0];
-	motor_inputs(1) = _actuators.control[1];
-	motor_inputs(2) = _actuators.control[2];
-	motor_inputs(3) = _actuators.control[3];
-}
-
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 515fb0c14a..009f762689 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -34,7 +34,7 @@
 
 /**
  * @file mc_att_control_base.h
- * 
+ *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -83,12 +83,7 @@ public:
 	 */
 	void control_attitude(float dt);
 	void control_attitude_rates(float dt);
-	
-	 // setters and getters for interface with euroc-gazebo simulator
-	void set_attitude(const Eigen::Quaternion<double> attitude);
-	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
-	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
-	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+
 	void set_actuator_controls();
 
 protected:
@@ -105,8 +100,6 @@ protected:
 	struct actuator_controls_s _actuators; /**< actuator controls */
 	struct actuator_armed_s _armed; /**< actuator arming status */
 
-	perf_counter_t _loop_perf; /**< loop performance counter */
-
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */
 	math::Vector<3> _rates_sp; /**< angular rates setpoint */
 	math::Vector<3> _rates_int; /**< angular rates integral error */
@@ -130,8 +123,10 @@ protected:
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
 	} _params;
-	
-	void vehicle_attitude_setpoint_poll();	//provisional
+
+	bool _publish_att_sp;
+
+	virtual void vehicle_attitude_setpoint_poll() = 0;
 
 };
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 81a13edb8f..7bc638e5d2 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -75,6 +75,7 @@
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
+#include "mc_att_control_base.h"
 
 /**
  * Multicopter attitude control app start / stop handling function
@@ -87,7 +88,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
-class MulticopterAttitudeControl
+class MulticopterAttitudeControl :
+	public MulticopterAttitudeControlBase
 {
 public:
 	/**
@@ -108,7 +110,6 @@ public:
 	int		start();
 
 private:
-
 	bool	_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
@@ -124,28 +125,6 @@ private:
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
-	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
-
-	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
-	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
-	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
-	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
-	struct actuator_controls_s			_actuators;			/**< actuator controls */
-	struct actuator_armed_s				_armed;				/**< actuator arming status */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
-	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
-	math::Vector<3>		_rates_int;		/**< angular rates integral error */
-	float				_thrust_sp;		/**< thrust setpoint */
-	math::Vector<3>		_att_control;	/**< attitude control vector */
-
-	math::Matrix<3, 3>  _I;				/**< identity matrix */
-
-	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
-
 	struct {
 		param_t roll_p;
 		param_t roll_rate_p;
@@ -170,19 +149,7 @@ private:
 		param_t acro_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
-	struct {
-		math::Vector<3> att_p;					/**< P gain for angular error */
-		math::Vector<3> rate_p;				/**< P gain for angular rate error */
-		math::Vector<3> rate_i;				/**< I gain for angular rate error */
-		math::Vector<3> rate_d;				/**< D gain for angular rate error */
-		float yaw_ff;						/**< yaw control feed-forward */
-		float yaw_rate_max;					/**< max yaw rate */
-
-		float man_roll_max;
-		float man_pitch_max;
-		float man_yaw_max;
-		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
-	}		_params;
+	perf_counter_t _loop_perf; /**< loop performance counter */
 
 	/**
 	 * Update our local parameter cache.
@@ -219,16 +186,6 @@ private:
 	 */
 	void		arming_status_poll();
 
-	/**
-	 * Attitude controller.
-	 */
-	void		control_attitude(float dt);
-
-	/**
-	 * Attitude rates controller.
-	 */
-	void		control_attitude_rates(float dt);
-
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -253,11 +210,8 @@ MulticopterAttitudeControl	*g_control;
 }
 
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
-
-	_task_should_exit(false),
-	_control_task(-1),
-
-/* subscriptions */
+	MulticopterAttitudeControlBase(),
+	/* subscriptions */
 	_v_att_sub(-1),
 	_v_att_sp_sub(-1),
 	_v_control_mode_sub(-1),
@@ -265,14 +219,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
 
-/* publications */
+	/* publications */
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
 
-	_actuators_0_circuit_breaker_enabled(false),
-
-/* performance counters */
+	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
@@ -489,229 +441,6 @@ MulticopterAttitudeControl::arming_status_poll()
 	}
 }
 
-/*
- * Attitude controller.
- * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
- * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
- */
-void
-MulticopterAttitudeControl::control_attitude(float dt)
-{
-	float yaw_sp_move_rate = 0.0f;
-	bool publish_att_sp = false;
-
-	if (_v_control_mode.flag_control_manual_enabled) {
-		/* manual input, set or modify attitude setpoint */
-
-		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
-			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			vehicle_attitude_setpoint_poll();
-		}
-
-		if (!_v_control_mode.flag_control_climb_rate_enabled) {
-			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.z;
-			publish_att_sp = true;
-		}
-
-		if (!_armed.armed) {
-			/* reset yaw setpoint when disarmed */
-			_reset_yaw_sp = true;
-		}
-
-		/* move yaw setpoint in all modes */
-		if (_v_att_sp.thrust < 0.1f) {
-			// TODO
-			//if (_status.condition_landed) {
-			/* reset yaw setpoint if on ground */
-			//	reset_yaw_sp = true;
-			//}
-		} else {
-			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
-			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
-			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
-			if (yaw_offs < - yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
-
-			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
-			}
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
-
-		/* reset yaw setpint to current position if needed */
-		if (_reset_yaw_sp) {
-			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att.yaw;
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
-
-		if (!_v_control_mode.flag_control_velocity_enabled) {
-			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
-
-	} else {
-		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		vehicle_attitude_setpoint_poll();
-
-		/* reset yaw setpoint after non-manual control mode */
-		_reset_yaw_sp = true;
-	}
-
-	_thrust_sp = _v_att_sp.thrust;
-
-	/* construct attitude setpoint rotation matrix */
-	math::Matrix<3, 3> R_sp;
-
-	if (_v_att_sp.R_valid) {
-		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0][0]);
-
-	} else {
-		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
-
-		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
-		_v_att_sp.R_valid = true;
-	}
-
-	/* publish the attitude setpoint if needed */
-	if (publish_att_sp) {
-		_v_att_sp.timestamp = hrt_absolute_time();
-
-		if (_att_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
-
-		} else {
-			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
-		}
-	}
-
-	/* rotation matrix for current state */
-	math::Matrix<3, 3> R;
-	R.set(_v_att.R);
-
-	/* all input data is ready, run controller itself */
-
-	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
-	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
-
-	/* axis and sin(angle) of desired rotation */
-	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
-
-	/* calculate angle error */
-	float e_R_z_sin = e_R.length();
-	float e_R_z_cos = R_z * R_sp_z;
-
-	/* calculate weight for yaw control */
-	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
-
-	/* calculate rotation matrix after roll/pitch only rotation */
-	math::Matrix<3, 3> R_rp;
-
-	if (e_R_z_sin > 0.0f) {
-		/* get axis-angle representation */
-		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
-		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
-
-		e_R = e_R_z_axis * e_R_z_angle;
-
-		/* cross product matrix for e_R_axis */
-		math::Matrix<3, 3> e_R_cp;
-		e_R_cp.zero();
-		e_R_cp(0, 1) = -e_R_z_axis(2);
-		e_R_cp(0, 2) = e_R_z_axis(1);
-		e_R_cp(1, 0) = e_R_z_axis(2);
-		e_R_cp(1, 2) = -e_R_z_axis(0);
-		e_R_cp(2, 0) = -e_R_z_axis(1);
-		e_R_cp(2, 1) = e_R_z_axis(0);
-
-		/* rotation matrix for roll/pitch only rotation */
-		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
-
-	} else {
-		/* zero roll/pitch rotation */
-		R_rp = R;
-	}
-
-	/* R_rp and R_sp has the same Z axis, calculate yaw error */
-	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
-	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
-	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
-
-	if (e_R_z_cos < 0.0f) {
-		/* for large thrust vector rotations use another rotation method:
-		 * calculate angle and axis for R -> R_sp rotation directly */
-		math::Quaternion q;
-		q.from_dcm(R.transposed() * R_sp);
-		math::Vector<3> e_R_d = q.imag();
-		e_R_d.normalize();
-		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
-
-		/* use fusion of Z axis based rotation and direct rotation */
-		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
-		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
-	}
-
-	/* calculate angular rates setpoint */
-	_rates_sp = _params.att_p.emult(e_R);
-
-	/* limit yaw rate */
-	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
-
-	/* feed forward yaw setpoint rate */
-	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
-}
-
-/*
- * Attitude rates controller.
- * Input: '_rates_sp' vector, '_thrust_sp'
- * Output: '_att_control' vector
- */
-void
-MulticopterAttitudeControl::control_attitude_rates(float dt)
-{
-	/* reset integral if disarmed */
-	if (!_armed.armed) {
-		_rates_int.zero();
-	}
-
-	/* current body angular rates */
-	math::Vector<3> rates;
-	rates(0) = _v_att.rollspeed;
-	rates(1) = _v_att.pitchspeed;
-	rates(2) = _v_att.yawspeed;
-
-	/* angular rates error */
-	math::Vector<3> rates_err = _rates_sp - rates;
-	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
-	_rates_prev = rates;
-
-	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
-		for (int i = 0; i < 3; i++) {
-			if (fabsf(_att_control(i)) < _thrust_sp) {
-				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
-
-				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
-					_rates_int(i) = rate_i;
-				}
-			}
-		}
-	}
-}
-
 void
 MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -787,6 +516,19 @@ MulticopterAttitudeControl::task_main()
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
 
+				/* publish the attitude setpoint if needed */
+				if (_publish_att_sp) {
+					_v_att_sp.timestamp = hrt_absolute_time();
+
+					if (_att_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+								&_v_att_sp);
+					} else {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+								&_v_att_sp);
+					}
+				}
+
 				/* publish attitude rates setpoint */
 				_v_rates_sp.roll = _rates_sp(0);
 				_v_rates_sp.pitch = _rates_sp(1);
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
new file mode 100644
index 0000000000..61a4237fc5
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -0,0 +1,107 @@
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_sim.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) {
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_v_att.q[0] = quat(0);
+	_v_att.q[1] = quat(1);
+	_v_att.q[2] = quat(2);
+	_v_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0,0);
+	_v_att.R[1][0] = Rot(1,0);
+	_v_att.R[2][0] = Rot(2,0);
+	_v_att.R[0][1] = Rot(0,1);
+	_v_att.R[1][1] = Rot(1,1);
+	_v_att.R[2][1] = Rot(2,1);
+	_v_att.R[0][2] = Rot(0,2);
+	_v_att.R[1][2] = Rot(1,2);
+	_v_att.R[2][2] = Rot(2,2);
+
+	_v_att.R_valid = true;
+}
+
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	// check if this is consistent !!!
+	_v_att.rollspeed  = angular_rate(0);
+	_v_att.pitchspeed = angular_rate(1);
+	_v_att.yawspeed   = angular_rate(2);
+}
+
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
+}
+
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
new file mode 100644
index 0000000000..20752b054d
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -0,0 +1,95 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.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/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+#inlcude "mc_att_control_base.h"
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlSim :
+	public MulticopterAttitudeControlBase
+
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlSim();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlSim();
+
+	/* setters and getters for interface with euroc-gazebo simulator */
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+
+protected:
+	void vehicle_attitude_setpoint_poll() {};
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk
index 64b876f69b..ecd251d45c 100644
--- a/src/modules/mc_att_control/module.mk
+++ b/src/modules/mc_att_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= mc_att_control
 
 SRCS		= mc_att_control_main.cpp \
-			  mc_att_control_params.c
+		  mc_att_control_base.cpp \
+		  mc_att_control_params.c

From 2f646e7082b49ef92bf9be1defd6fb7fdeec8480 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:02:06 +0100
Subject: [PATCH 118/416] mc att control: ran fix code style script

---
 .../mc_att_control/mc_att_control_base.cpp    | 36 +++++++-----
 .../mc_att_control/mc_att_control_base.h      |  3 +-
 .../mc_att_control/mc_att_control_main.cpp    | 23 +++++---
 .../mc_att_control/mc_att_control_sim.cpp     | 56 ++++++++++---------
 .../mc_att_control/mc_att_control_sim.h       |  6 +-
 5 files changed, 71 insertions(+), 53 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index d21deb7157..a3b0340d2e 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_I.identity();
 }
 
-MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
+{
 }
 
-void MulticopterAttitudeControlBase::control_attitude(float dt) {
+void MulticopterAttitudeControlBase::control_attitude(float dt)
+{
 	float yaw_sp_move_rate = 0.0f;
 	_publish_att_sp = false;
 
@@ -94,7 +96,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 		/* manual input, set or modify attitude setpoint */
 
 		if (_v_control_mode.flag_control_velocity_enabled
-				|| _v_control_mode.flag_control_climb_rate_enabled) {
+		    || _v_control_mode.flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
 			vehicle_attitude_setpoint_poll();
 		}
@@ -121,15 +123,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			/* move yaw setpoint */
 			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(
-					_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+						     _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
 			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+
 			if (yaw_offs < -yaw_offs_max) {
 				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
 				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
 			}
+
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
 		}
@@ -146,7 +150,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			/* update attitude setpoint if not in position control mode */
 			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
 			_v_att_sp.pitch_body = -_manual_control_sp.x
-					* _params.man_pitch_max;
+					       * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
 		}
@@ -175,7 +179,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 
 		/* copy rotation matrix back to setpoint struct */
 		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
-				sizeof(_v_att_sp.R_body));
+		       sizeof(_v_att_sp.R_body));
 		_v_att_sp.R_valid = true;
 	}
 
@@ -221,8 +225,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 
 		/* rotation matrix for roll/pitch only rotation */
 		R_rp = R
-				* (_I + e_R_cp * e_R_z_sin
-						+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+		       * (_I + e_R_cp * e_R_z_sin
+			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 	} else {
 		/* zero roll/pitch rotation */
@@ -253,13 +257,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 
 	/* limit yaw rate */
 	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
-			_params.yaw_rate_max);
+				       _params.yaw_rate_max);
 
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 }
 
-void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
+{
 	/* reset integral if disarmed */
 	if (!_armed.armed) {
 		_rates_int.zero();
@@ -274,7 +279,7 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 	/* angular rates error */
 	math::Vector < 3 > rates_err = _rates_sp - rates;
 	_att_control = _params.rate_p.emult(rates_err)
-			+ _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+		       + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
 	_rates_prev = rates;
 
 	/* update integral only if not saturated on low limit */
@@ -282,11 +287,11 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i)
-						+ _params.rate_i(i) * rates_err(i) * dt;
+					       + _params.rate_i(i) * rates_err(i) * dt;
 
 				if (isfinite(
-						rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-						_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					    rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
 					_rates_int(i) = rate_i;
 				}
 			}
@@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 
 }
 
-void MulticopterAttitudeControlBase::set_actuator_controls() {
+void MulticopterAttitudeControlBase::set_actuator_controls()
+{
 	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
 	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
 	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 009f762689..e7793e624c 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -64,7 +64,8 @@
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
-class MulticopterAttitudeControlBase {
+class MulticopterAttitudeControlBase
+{
 public:
 	/**
 	 * Constructor
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 7bc638e5d2..d6a58f4186 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -477,8 +477,9 @@ MulticopterAttitudeControl::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -522,10 +523,11 @@ MulticopterAttitudeControl::task_main()
 
 					if (_att_sp_pub > 0) {
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-								&_v_att_sp);
+							    &_v_att_sp);
+
 					} else {
 						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-								&_v_att_sp);
+									    &_v_att_sp);
 					}
 				}
 
@@ -547,7 +549,8 @@ MulticopterAttitudeControl::task_main()
 				/* attitude controller disabled, poll rates setpoint topic */
 				if (_v_control_mode.flag_control_manual_enabled) {
 					/* manual rates control - ACRO mode */
-					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
+								    _manual_control_sp.r).emult(_params.acro_rate_max);
 					_thrust_sp = _manual_control_sp.z;
 
 					/* reset yaw setpoint after ACRO */
@@ -630,18 +633,21 @@ MulticopterAttitudeControl::start()
 
 int mc_att_control_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: mc_att_control {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (mc_att_control::g_control != nullptr)
+		if (mc_att_control::g_control != nullptr) {
 			errx(1, "already running");
+		}
 
 		mc_att_control::g_control = new MulticopterAttitudeControl;
 
-		if (mc_att_control::g_control == nullptr)
+		if (mc_att_control::g_control == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != mc_att_control::g_control->start()) {
 			delete mc_att_control::g_control;
@@ -653,8 +659,9 @@ int mc_att_control_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (mc_att_control::g_control == nullptr)
+		if (mc_att_control::g_control == nullptr) {
 			errx(1, "not running");
+		}
 
 		delete mc_att_control::g_control;
 		mc_att_control::g_control = nullptr;
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index 61a4237fc5..faf7294208 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -46,7 +46,8 @@
 using namespace std;
 #endif
 
-void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) {
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
+{
 	math::Quaternion quat;
 	quat(0) = (float)attitude.w();
 	quat(1) = (float)attitude.x();
@@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double>
 	_v_att.q[2] = quat(2);
 	_v_att.q[3] = quat(3);
 
-	math::Matrix<3,3> Rot = quat.to_dcm();
-	_v_att.R[0][0] = Rot(0,0);
-	_v_att.R[1][0] = Rot(1,0);
-	_v_att.R[2][0] = Rot(2,0);
-	_v_att.R[0][1] = Rot(0,1);
-	_v_att.R[1][1] = Rot(1,1);
-	_v_att.R[2][1] = Rot(2,1);
-	_v_att.R[0][2] = Rot(0,2);
-	_v_att.R[1][2] = Rot(1,2);
-	_v_att.R[2][2] = Rot(2,2);
+	math::Matrix<3, 3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0, 0);
+	_v_att.R[1][0] = Rot(1, 0);
+	_v_att.R[2][0] = Rot(2, 0);
+	_v_att.R[0][1] = Rot(0, 1);
+	_v_att.R[1][1] = Rot(1, 1);
+	_v_att.R[2][1] = Rot(2, 1);
+	_v_att.R[0][2] = Rot(0, 2);
+	_v_att.R[1][2] = Rot(1, 2);
+	_v_att.R[2][2] = Rot(2, 2);
 
 	_v_att.R_valid = true;
 }
 
-void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
+{
 	// check if this is consistent !!!
 	_v_att.rollspeed  = angular_rate(0);
 	_v_att.pitchspeed = angular_rate(1);
 	_v_att.yawspeed   = angular_rate(2);
 }
 
-void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
+{
 	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
 	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
 	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) - 30) * (-1) / 30;
 
 	// setup rotation matrix
-	math::Matrix<3,3> Rot_sp;
-	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
-	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
-	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
-	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
-	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
-	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
-	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
-	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
-	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
-	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
+	math::Matrix<3, 3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0, 0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1, 0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2, 0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0, 1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1, 1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2, 1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0, 2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1, 2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2, 2);
 }
 
-void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
+{
 	motor_inputs(0) = _actuators.control[0];
 	motor_inputs(1) = _actuators.control[1];
 	motor_inputs(2) = _actuators.control[2];
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
index 20752b054d..0b48455af0 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.h
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -82,9 +82,9 @@ public:
 
 	/* setters and getters for interface with euroc-gazebo simulator */
 	void set_attitude(const Eigen::Quaternion<double> attitude);
-	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
-	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
-	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	void set_attitude_rates(const Eigen::Vector3d &angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d &motor_inputs);
 
 protected:
 	void vehicle_attitude_setpoint_poll() {};

From eafc4b5f9eefabca5fba404d75b1e1425b2f579b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 08:57:24 +0100
Subject: [PATCH 119/416] mc att control: make the main app use the base class,
 move euroc functions into own class

---
 .../mc_att_control/mc_att_control_base.cpp    | 120 +------
 .../mc_att_control/mc_att_control_base.h      |  17 +-
 .../mc_att_control/mc_att_control_main.cpp    | 300 ++----------------
 .../mc_att_control/mc_att_control_sim.cpp     | 107 +++++++
 .../mc_att_control/mc_att_control_sim.h       |  95 ++++++
 src/modules/mc_att_control/module.mk          |   3 +-
 6 files changed, 241 insertions(+), 401 deletions(-)
 create mode 100644 src/modules/mc_att_control/mc_att_control_sim.cpp
 create mode 100644 src/modules/mc_att_control/mc_att_control_sim.h

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index baf2bfe65c..d21deb7157 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -31,7 +31,7 @@
 
 /**
  * @file mc_att_control_base.h
- * 
+ *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -47,13 +47,12 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+	_task_should_exit(false),
+	_control_task(-1),
 
-		_task_should_exit(false), _control_task(-1),
+	_actuators_0_circuit_breaker_enabled(false),
 
-		_actuators_0_circuit_breaker_enabled(false),
-
-		/* performance counters */
-		_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+	_publish_att_sp(false)
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
@@ -82,38 +81,14 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_att_control.zero();
 
 	_I.identity();
-
-	// setup standard gains
-	_params.att_p(0)      = 5.0;
-	_params.rate_p(0)     = 0.05;
-	_params.rate_i(0)     = 0.0;
-	_params.rate_d(0)     = 0.003;
-	/* pitch gains */
-	_params.att_p(1)      = 5.0;
-	_params.rate_p(1)     = 0.05;
-	_params.rate_i(1)     = 0.0;
-	_params.rate_d(1)     = 0.003;
-	/* yaw gains */
-	_params.att_p(2)      = 2.8;
-	_params.rate_p(2)     = 0.2;
-	_params.rate_i(2)     = 0.1;
-	_params.rate_d(2)     = 0.0;
-	_params.yaw_rate_max  = 0.5;
-	_params.yaw_ff        = 0.5;
-	_params.man_roll_max  = 0.6;
-	_params.man_pitch_max = 0.6;
-	_params.man_yaw_max   = 0.6;
 }
 
 MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
 }
 
-void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() {
-}
-
 void MulticopterAttitudeControlBase::control_attitude(float dt) {
 	float yaw_sp_move_rate = 0.0f;
-	bool publish_att_sp = false;
+	_publish_att_sp = false;
 
 	if (_v_control_mode.flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
@@ -127,7 +102,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 		if (!_v_control_mode.flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
 			_v_att_sp.thrust = _manual_control_sp.z;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 		if (!_armed.armed) {
@@ -156,7 +131,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
 			}
 			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 		/* reset yaw setpint to current position if needed */
@@ -164,7 +139,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			_reset_yaw_sp = false;
 			_v_att_sp.yaw_body = _v_att.yaw;
 			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 		if (!_v_control_mode.flag_control_velocity_enabled) {
@@ -173,7 +148,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			_v_att_sp.pitch_body = -_manual_control_sp.x
 					* _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
+			_publish_att_sp = true;
 		}
 
 	} else {
@@ -204,20 +179,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 		_v_att_sp.R_valid = true;
 	}
 
-//	/* publish the attitude setpoint if needed */
-//	if (publish_att_sp) {
-//		_v_att_sp.timestamp = hrt_absolute_time();
-//
-//		if (_att_sp_pub > 0) {
-//			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-//					&_v_att_sp);
-//
-//		} else {
-//			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-//					&_v_att_sp);
-//		}
-//	}
-
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
 	R.set(_v_att.R);
@@ -340,64 +301,3 @@ void MulticopterAttitudeControlBase::set_actuator_controls() {
 	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 }
-
-void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
-	math::Quaternion quat;
-	quat(0) = (float)attitude.w();
-	quat(1) = (float)attitude.x();
-	quat(2) = (float)attitude.y();
-	quat(3) = (float)attitude.z();
-
-	_v_att.q[0] = quat(0);
-	_v_att.q[1] = quat(1);
-	_v_att.q[2] = quat(2);
-	_v_att.q[3] = quat(3);
-
-	math::Matrix<3,3> Rot = quat.to_dcm();
-	_v_att.R[0][0] = Rot(0,0);
-	_v_att.R[1][0] = Rot(1,0);
-	_v_att.R[2][0] = Rot(2,0);
-	_v_att.R[0][1] = Rot(0,1);
-	_v_att.R[1][1] = Rot(1,1);
-	_v_att.R[2][1] = Rot(2,1);
-	_v_att.R[0][2] = Rot(0,2);
-	_v_att.R[1][2] = Rot(1,2);
-	_v_att.R[2][2] = Rot(2,2);
-
-	_v_att.R_valid = true;
-}
-
-void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
-	// check if this is consistent !!!
-	_v_att.rollspeed  = angular_rate(0);
-	_v_att.pitchspeed = angular_rate(1);
-	_v_att.yawspeed   = angular_rate(2);
-}
-
-void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
-	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
-	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
-	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
-
-	// setup rotation matrix
-	math::Matrix<3,3> Rot_sp;
-	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
-	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
-	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
-	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
-	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
-	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
-	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
-	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
-	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
-	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
-}
-
-void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
-	motor_inputs(0) = _actuators.control[0];
-	motor_inputs(1) = _actuators.control[1];
-	motor_inputs(2) = _actuators.control[2];
-	motor_inputs(3) = _actuators.control[3];
-}
-
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 515fb0c14a..009f762689 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -34,7 +34,7 @@
 
 /**
  * @file mc_att_control_base.h
- * 
+ *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -83,12 +83,7 @@ public:
 	 */
 	void control_attitude(float dt);
 	void control_attitude_rates(float dt);
-	
-	 // setters and getters for interface with euroc-gazebo simulator
-	void set_attitude(const Eigen::Quaternion<double> attitude);
-	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
-	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
-	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+
 	void set_actuator_controls();
 
 protected:
@@ -105,8 +100,6 @@ protected:
 	struct actuator_controls_s _actuators; /**< actuator controls */
 	struct actuator_armed_s _armed; /**< actuator arming status */
 
-	perf_counter_t _loop_perf; /**< loop performance counter */
-
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */
 	math::Vector<3> _rates_sp; /**< angular rates setpoint */
 	math::Vector<3> _rates_int; /**< angular rates integral error */
@@ -130,8 +123,10 @@ protected:
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
 	} _params;
-	
-	void vehicle_attitude_setpoint_poll();	//provisional
+
+	bool _publish_att_sp;
+
+	virtual void vehicle_attitude_setpoint_poll() = 0;
 
 };
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 19c10198c2..4709f0487b 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -75,6 +75,7 @@
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
+#include "mc_att_control_base.h"
 
 /**
  * Multicopter attitude control app start / stop handling function
@@ -87,7 +88,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
-class MulticopterAttitudeControl
+class MulticopterAttitudeControl :
+	public MulticopterAttitudeControlBase
 {
 public:
 	/**
@@ -108,7 +110,6 @@ public:
 	int		start();
 
 private:
-
 	bool	_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
@@ -124,28 +125,6 @@ private:
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
-	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
-
-	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
-	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
-	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
-	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
-	struct actuator_controls_s			_actuators;			/**< actuator controls */
-	struct actuator_armed_s				_armed;				/**< actuator arming status */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
-	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
-	math::Vector<3>		_rates_int;		/**< angular rates integral error */
-	float				_thrust_sp;		/**< thrust setpoint */
-	math::Vector<3>		_att_control;	/**< attitude control vector */
-
-	math::Matrix<3, 3>  _I;				/**< identity matrix */
-
-	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
-
 	struct {
 		param_t roll_p;
 		param_t roll_rate_p;
@@ -170,19 +149,7 @@ private:
 		param_t acro_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
-	struct {
-		math::Vector<3> att_p;					/**< P gain for angular error */
-		math::Vector<3> rate_p;				/**< P gain for angular rate error */
-		math::Vector<3> rate_i;				/**< I gain for angular rate error */
-		math::Vector<3> rate_d;				/**< D gain for angular rate error */
-		float yaw_ff;						/**< yaw control feed-forward */
-		float yaw_rate_max;					/**< max yaw rate */
-
-		float man_roll_max;
-		float man_pitch_max;
-		float man_yaw_max;
-		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
-	}		_params;
+	perf_counter_t _loop_perf; /**< loop performance counter */
 
 	/**
 	 * Update our local parameter cache.
@@ -219,16 +186,6 @@ private:
 	 */
 	void		arming_status_poll();
 
-	/**
-	 * Attitude controller.
-	 */
-	void		control_attitude(float dt);
-
-	/**
-	 * Attitude rates controller.
-	 */
-	void		control_attitude_rates(float dt);
-
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -253,11 +210,8 @@ MulticopterAttitudeControl	*g_control;
 }
 
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
-
-	_task_should_exit(false),
-	_control_task(-1),
-
-/* subscriptions */
+	MulticopterAttitudeControlBase(),
+	/* subscriptions */
 	_v_att_sub(-1),
 	_v_att_sp_sub(-1),
 	_v_control_mode_sub(-1),
@@ -265,14 +219,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
 
-/* publications */
+	/* publications */
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
 
-	_actuators_0_circuit_breaker_enabled(false),
-
-/* performance counters */
+	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
@@ -489,229 +441,6 @@ MulticopterAttitudeControl::arming_status_poll()
 	}
 }
 
-/*
- * Attitude controller.
- * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
- * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
- */
-void
-MulticopterAttitudeControl::control_attitude(float dt)
-{
-	float yaw_sp_move_rate = 0.0f;
-	bool publish_att_sp = false;
-
-	if (_v_control_mode.flag_control_manual_enabled) {
-		/* manual input, set or modify attitude setpoint */
-
-		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
-			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			vehicle_attitude_setpoint_poll();
-		}
-
-		if (!_v_control_mode.flag_control_climb_rate_enabled) {
-			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.z;
-			publish_att_sp = true;
-		}
-
-		if (!_armed.armed) {
-			/* reset yaw setpoint when disarmed */
-			_reset_yaw_sp = true;
-		}
-
-		/* move yaw setpoint in all modes */
-		if (_v_att_sp.thrust < 0.1f) {
-			// TODO
-			//if (_status.condition_landed) {
-			/* reset yaw setpoint if on ground */
-			//	reset_yaw_sp = true;
-			//}
-		} else {
-			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
-			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
-			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
-			if (yaw_offs < - yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
-
-			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
-			}
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
-
-		/* reset yaw setpint to current position if needed */
-		if (_reset_yaw_sp) {
-			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att.yaw;
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
-
-		if (!_v_control_mode.flag_control_velocity_enabled) {
-			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
-
-	} else {
-		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		vehicle_attitude_setpoint_poll();
-
-		/* reset yaw setpoint after non-manual control mode */
-		_reset_yaw_sp = true;
-	}
-
-	_thrust_sp = _v_att_sp.thrust;
-
-	/* construct attitude setpoint rotation matrix */
-	math::Matrix<3, 3> R_sp;
-
-	if (_v_att_sp.R_valid) {
-		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0][0]);
-
-	} else {
-		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
-
-		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
-		_v_att_sp.R_valid = true;
-	}
-
-	/* publish the attitude setpoint if needed */
-	if (publish_att_sp) {
-		_v_att_sp.timestamp = hrt_absolute_time();
-
-		if (_att_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
-
-		} else {
-			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
-		}
-	}
-
-	/* rotation matrix for current state */
-	math::Matrix<3, 3> R;
-	R.set(_v_att.R);
-
-	/* all input data is ready, run controller itself */
-
-	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
-	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
-
-	/* axis and sin(angle) of desired rotation */
-	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
-
-	/* calculate angle error */
-	float e_R_z_sin = e_R.length();
-	float e_R_z_cos = R_z * R_sp_z;
-
-	/* calculate weight for yaw control */
-	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
-
-	/* calculate rotation matrix after roll/pitch only rotation */
-	math::Matrix<3, 3> R_rp;
-
-	if (e_R_z_sin > 0.0f) {
-		/* get axis-angle representation */
-		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
-		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
-
-		e_R = e_R_z_axis * e_R_z_angle;
-
-		/* cross product matrix for e_R_axis */
-		math::Matrix<3, 3> e_R_cp;
-		e_R_cp.zero();
-		e_R_cp(0, 1) = -e_R_z_axis(2);
-		e_R_cp(0, 2) = e_R_z_axis(1);
-		e_R_cp(1, 0) = e_R_z_axis(2);
-		e_R_cp(1, 2) = -e_R_z_axis(0);
-		e_R_cp(2, 0) = -e_R_z_axis(1);
-		e_R_cp(2, 1) = e_R_z_axis(0);
-
-		/* rotation matrix for roll/pitch only rotation */
-		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
-
-	} else {
-		/* zero roll/pitch rotation */
-		R_rp = R;
-	}
-
-	/* R_rp and R_sp has the same Z axis, calculate yaw error */
-	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
-	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
-	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
-
-	if (e_R_z_cos < 0.0f) {
-		/* for large thrust vector rotations use another rotation method:
-		 * calculate angle and axis for R -> R_sp rotation directly */
-		math::Quaternion q;
-		q.from_dcm(R.transposed() * R_sp);
-		math::Vector<3> e_R_d = q.imag();
-		e_R_d.normalize();
-		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
-
-		/* use fusion of Z axis based rotation and direct rotation */
-		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
-		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
-	}
-
-	/* calculate angular rates setpoint */
-	_rates_sp = _params.att_p.emult(e_R);
-
-	/* limit yaw rate */
-	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
-
-	/* feed forward yaw setpoint rate */
-	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
-}
-
-/*
- * Attitude rates controller.
- * Input: '_rates_sp' vector, '_thrust_sp'
- * Output: '_att_control' vector
- */
-void
-MulticopterAttitudeControl::control_attitude_rates(float dt)
-{
-	/* reset integral if disarmed */
-	if (!_armed.armed) {
-		_rates_int.zero();
-	}
-
-	/* current body angular rates */
-	math::Vector<3> rates;
-	rates(0) = _v_att.rollspeed;
-	rates(1) = _v_att.pitchspeed;
-	rates(2) = _v_att.yawspeed;
-
-	/* angular rates error */
-	math::Vector<3> rates_err = _rates_sp - rates;
-	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
-	_rates_prev = rates;
-
-	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
-		for (int i = 0; i < 3; i++) {
-			if (fabsf(_att_control(i)) < _thrust_sp) {
-				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
-
-				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
-					_rates_int(i) = rate_i;
-				}
-			}
-		}
-	}
-}
-
 void
 MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -789,6 +518,19 @@ MulticopterAttitudeControl::task_main()
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
 
+				/* publish the attitude setpoint if needed */
+				if (_publish_att_sp) {
+					_v_att_sp.timestamp = hrt_absolute_time();
+
+					if (_att_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+								&_v_att_sp);
+					} else {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+								&_v_att_sp);
+					}
+				}
+
 				/* publish attitude rates setpoint */
 				_v_rates_sp.roll = _rates_sp(0);
 				_v_rates_sp.pitch = _rates_sp(1);
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
new file mode 100644
index 0000000000..61a4237fc5
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -0,0 +1,107 @@
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_sim.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) {
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_v_att.q[0] = quat(0);
+	_v_att.q[1] = quat(1);
+	_v_att.q[2] = quat(2);
+	_v_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0,0);
+	_v_att.R[1][0] = Rot(1,0);
+	_v_att.R[2][0] = Rot(2,0);
+	_v_att.R[0][1] = Rot(0,1);
+	_v_att.R[1][1] = Rot(1,1);
+	_v_att.R[2][1] = Rot(2,1);
+	_v_att.R[0][2] = Rot(0,2);
+	_v_att.R[1][2] = Rot(1,2);
+	_v_att.R[2][2] = Rot(2,2);
+
+	_v_att.R_valid = true;
+}
+
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	// check if this is consistent !!!
+	_v_att.rollspeed  = angular_rate(0);
+	_v_att.pitchspeed = angular_rate(1);
+	_v_att.yawspeed   = angular_rate(2);
+}
+
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
+}
+
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
new file mode 100644
index 0000000000..20752b054d
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -0,0 +1,95 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.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/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+#inlcude "mc_att_control_base.h"
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlSim :
+	public MulticopterAttitudeControlBase
+
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlSim();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlSim();
+
+	/* setters and getters for interface with euroc-gazebo simulator */
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+
+protected:
+	void vehicle_attitude_setpoint_poll() {};
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk
index 64b876f69b..ecd251d45c 100644
--- a/src/modules/mc_att_control/module.mk
+++ b/src/modules/mc_att_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= mc_att_control
 
 SRCS		= mc_att_control_main.cpp \
-			  mc_att_control_params.c
+		  mc_att_control_base.cpp \
+		  mc_att_control_params.c

From 1c19d75cf429639ff9ab9ff23ad9fbcd1c936e98 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:02:06 +0100
Subject: [PATCH 120/416] mc att control: ran fix code style script

---
 .../mc_att_control/mc_att_control_base.cpp    | 36 +++++++-----
 .../mc_att_control/mc_att_control_base.h      |  3 +-
 .../mc_att_control/mc_att_control_main.cpp    | 23 +++++---
 .../mc_att_control/mc_att_control_sim.cpp     | 56 ++++++++++---------
 .../mc_att_control/mc_att_control_sim.h       |  6 +-
 5 files changed, 71 insertions(+), 53 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index d21deb7157..a3b0340d2e 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_I.identity();
 }
 
-MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
+{
 }
 
-void MulticopterAttitudeControlBase::control_attitude(float dt) {
+void MulticopterAttitudeControlBase::control_attitude(float dt)
+{
 	float yaw_sp_move_rate = 0.0f;
 	_publish_att_sp = false;
 
@@ -94,7 +96,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 		/* manual input, set or modify attitude setpoint */
 
 		if (_v_control_mode.flag_control_velocity_enabled
-				|| _v_control_mode.flag_control_climb_rate_enabled) {
+		    || _v_control_mode.flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
 			vehicle_attitude_setpoint_poll();
 		}
@@ -121,15 +123,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			/* move yaw setpoint */
 			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(
-					_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+						     _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
 			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+
 			if (yaw_offs < -yaw_offs_max) {
 				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
 				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
 			}
+
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
 		}
@@ -146,7 +150,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 			/* update attitude setpoint if not in position control mode */
 			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
 			_v_att_sp.pitch_body = -_manual_control_sp.x
-					* _params.man_pitch_max;
+					       * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
 		}
@@ -175,7 +179,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 
 		/* copy rotation matrix back to setpoint struct */
 		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
-				sizeof(_v_att_sp.R_body));
+		       sizeof(_v_att_sp.R_body));
 		_v_att_sp.R_valid = true;
 	}
 
@@ -221,8 +225,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 
 		/* rotation matrix for roll/pitch only rotation */
 		R_rp = R
-				* (_I + e_R_cp * e_R_z_sin
-						+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+		       * (_I + e_R_cp * e_R_z_sin
+			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 	} else {
 		/* zero roll/pitch rotation */
@@ -253,13 +257,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
 
 	/* limit yaw rate */
 	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
-			_params.yaw_rate_max);
+				       _params.yaw_rate_max);
 
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 }
 
-void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
+{
 	/* reset integral if disarmed */
 	if (!_armed.armed) {
 		_rates_int.zero();
@@ -274,7 +279,7 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 	/* angular rates error */
 	math::Vector < 3 > rates_err = _rates_sp - rates;
 	_att_control = _params.rate_p.emult(rates_err)
-			+ _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+		       + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
 	_rates_prev = rates;
 
 	/* update integral only if not saturated on low limit */
@@ -282,11 +287,11 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i)
-						+ _params.rate_i(i) * rates_err(i) * dt;
+					       + _params.rate_i(i) * rates_err(i) * dt;
 
 				if (isfinite(
-						rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-						_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					    rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
 					_rates_int(i) = rate_i;
 				}
 			}
@@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
 
 }
 
-void MulticopterAttitudeControlBase::set_actuator_controls() {
+void MulticopterAttitudeControlBase::set_actuator_controls()
+{
 	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
 	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
 	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 009f762689..e7793e624c 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -64,7 +64,8 @@
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
-class MulticopterAttitudeControlBase {
+class MulticopterAttitudeControlBase
+{
 public:
 	/**
 	 * Constructor
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 4709f0487b..7a03553f9c 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -479,8 +479,9 @@ MulticopterAttitudeControl::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -524,10 +525,11 @@ MulticopterAttitudeControl::task_main()
 
 					if (_att_sp_pub > 0) {
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-								&_v_att_sp);
+							    &_v_att_sp);
+
 					} else {
 						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-								&_v_att_sp);
+									    &_v_att_sp);
 					}
 				}
 
@@ -549,7 +551,8 @@ MulticopterAttitudeControl::task_main()
 				/* attitude controller disabled, poll rates setpoint topic */
 				if (_v_control_mode.flag_control_manual_enabled) {
 					/* manual rates control - ACRO mode */
-					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
+								    _manual_control_sp.r).emult(_params.acro_rate_max);
 					_thrust_sp = _manual_control_sp.z;
 
 					/* reset yaw setpoint after ACRO */
@@ -632,18 +635,21 @@ MulticopterAttitudeControl::start()
 
 int mc_att_control_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: mc_att_control {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (mc_att_control::g_control != nullptr)
+		if (mc_att_control::g_control != nullptr) {
 			errx(1, "already running");
+		}
 
 		mc_att_control::g_control = new MulticopterAttitudeControl;
 
-		if (mc_att_control::g_control == nullptr)
+		if (mc_att_control::g_control == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != mc_att_control::g_control->start()) {
 			delete mc_att_control::g_control;
@@ -655,8 +661,9 @@ int mc_att_control_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (mc_att_control::g_control == nullptr)
+		if (mc_att_control::g_control == nullptr) {
 			errx(1, "not running");
+		}
 
 		delete mc_att_control::g_control;
 		mc_att_control::g_control = nullptr;
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index 61a4237fc5..faf7294208 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -46,7 +46,8 @@
 using namespace std;
 #endif
 
-void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) {
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
+{
 	math::Quaternion quat;
 	quat(0) = (float)attitude.w();
 	quat(1) = (float)attitude.x();
@@ -58,48 +59,51 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double>
 	_v_att.q[2] = quat(2);
 	_v_att.q[3] = quat(3);
 
-	math::Matrix<3,3> Rot = quat.to_dcm();
-	_v_att.R[0][0] = Rot(0,0);
-	_v_att.R[1][0] = Rot(1,0);
-	_v_att.R[2][0] = Rot(2,0);
-	_v_att.R[0][1] = Rot(0,1);
-	_v_att.R[1][1] = Rot(1,1);
-	_v_att.R[2][1] = Rot(2,1);
-	_v_att.R[0][2] = Rot(0,2);
-	_v_att.R[1][2] = Rot(1,2);
-	_v_att.R[2][2] = Rot(2,2);
+	math::Matrix<3, 3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0, 0);
+	_v_att.R[1][0] = Rot(1, 0);
+	_v_att.R[2][0] = Rot(2, 0);
+	_v_att.R[0][1] = Rot(0, 1);
+	_v_att.R[1][1] = Rot(1, 1);
+	_v_att.R[2][1] = Rot(2, 1);
+	_v_att.R[0][2] = Rot(0, 2);
+	_v_att.R[1][2] = Rot(1, 2);
+	_v_att.R[2][2] = Rot(2, 2);
 
 	_v_att.R_valid = true;
 }
 
-void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
+{
 	// check if this is consistent !!!
 	_v_att.rollspeed  = angular_rate(0);
 	_v_att.pitchspeed = angular_rate(1);
 	_v_att.yawspeed   = angular_rate(2);
 }
 
-void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
+{
 	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
 	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
 	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) - 30) * (-1) / 30;
 
 	// setup rotation matrix
-	math::Matrix<3,3> Rot_sp;
-	Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body);
-	_v_att_sp.R_body[0][0] = Rot_sp(0,0);
-	_v_att_sp.R_body[1][0] = Rot_sp(1,0);
-	_v_att_sp.R_body[2][0] = Rot_sp(2,0);
-	_v_att_sp.R_body[0][1] = Rot_sp(0,1);
-	_v_att_sp.R_body[1][1] = Rot_sp(1,1);
-	_v_att_sp.R_body[2][1] = Rot_sp(2,1);
-	_v_att_sp.R_body[0][2] = Rot_sp(0,2);
-	_v_att_sp.R_body[1][2] = Rot_sp(1,2);
-	_v_att_sp.R_body[2][2] = Rot_sp(2,2);
+	math::Matrix<3, 3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0, 0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1, 0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2, 0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0, 1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1, 1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2, 1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0, 2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1, 2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2, 2);
 }
 
-void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
+{
 	motor_inputs(0) = _actuators.control[0];
 	motor_inputs(1) = _actuators.control[1];
 	motor_inputs(2) = _actuators.control[2];
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
index 20752b054d..0b48455af0 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.h
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -82,9 +82,9 @@ public:
 
 	/* setters and getters for interface with euroc-gazebo simulator */
 	void set_attitude(const Eigen::Quaternion<double> attitude);
-	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
-	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
-	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	void set_attitude_rates(const Eigen::Vector3d &angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d &motor_inputs);
 
 protected:
 	void vehicle_attitude_setpoint_poll() {};

From 85aa6554ad90e392a9538006f8cc483d481f0bbd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:29:20 +0100
Subject: [PATCH 121/416] re-add hardcoded parameters for euroc

---
 .../mc_att_control/mc_att_control_sim.cpp     | 29 +++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index faf7294208..a8467f6b12 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -46,6 +46,35 @@
 using namespace std;
 #endif
 
+MulticopterAttitudeControlSim::MulticopterAttitudeControlSim()
+{
+	/* setup standard gains */
+	//XXX: make these configurable
+	_params.att_p(0)      = 5.0;
+	_params.rate_p(0)     = 0.05;
+	_params.rate_i(0)     = 0.0;
+	_params.rate_d(0)     = 0.003;
+	/* pitch gains */
+	_params.att_p(1)      = 5.0;
+	_params.rate_p(1)     = 0.05;
+	_params.rate_i(1)     = 0.0;
+	_params.rate_d(1)     = 0.003;
+	/* yaw gains */
+	_params.att_p(2)      = 2.8;
+	_params.rate_p(2)     = 0.2;
+	_params.rate_i(2)     = 0.1;
+	_params.rate_d(2)     = 0.0;
+	_params.yaw_rate_max  = 0.5;
+	_params.yaw_ff        = 0.5;
+	_params.man_roll_max  = 0.6;
+	_params.man_pitch_max = 0.6;
+	_params.man_yaw_max   = 0.6;
+}
+
+MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim()
+{
+}
+
 void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
 {
 	math::Quaternion quat;

From 417bc91363c2607906c612089cb4b7197a60f531 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:29:57 +0100
Subject: [PATCH 122/416] mc att: more cleanup between base and main class

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 10 +++++-----
 src/modules/mc_att_control/mc_att_control_base.h   | 10 +++++-----
 src/modules/mc_att_control/mc_att_control_main.cpp | 12 ++++++++++--
 3 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index a3b0340d2e..305fbaf892 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -32,6 +32,11 @@
 /**
  * @file mc_att_control_base.h
  *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -47,11 +52,6 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
-	_task_should_exit(false),
-	_control_task(-1),
-
-	_actuators_0_circuit_breaker_enabled(false),
-
 	_publish_att_sp(false)
 
 {
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index e7793e624c..662d32ee44 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -35,6 +35,11 @@
 /**
  * @file mc_att_control_base.h
  *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -88,11 +93,6 @@ public:
 	void set_actuator_controls();
 
 protected:
-
-	bool _task_should_exit; /**< if true, sensor task should exit */
-	int _control_task; /**< task handle for sensor task */
-	bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
-
 	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index d6a58f4186..887a535088 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -38,6 +38,9 @@
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
  * The controller has two loops: P loop for angular error and PD loop for angular rate error.
  * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
@@ -110,8 +113,10 @@ public:
 	int		start();
 
 private:
-	bool	_task_should_exit;		/**< if true, sensor task should exit */
-	int		_control_task;			/**< task handle for sensor task */
+	bool		_task_should_exit;			/**< if true, sensor task should exit */
+	int		_control_task;				/**< task handle for sensor task */
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
 
 	int		_v_att_sub;				/**< vehicle attitude subscription */
 	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
@@ -211,6 +216,9 @@ MulticopterAttitudeControl	*g_control;
 
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	MulticopterAttitudeControlBase(),
+	_task_should_exit(false),
+	_control_task(-1),
+	_actuators_0_circuit_breaker_enabled(false),
 	/* subscriptions */
 	_v_att_sub(-1),
 	_v_att_sp_sub(-1),

From dfd2098e04761ee65ab09a9a8e6656a334ccae41 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:29:20 +0100
Subject: [PATCH 123/416] re-add hardcoded parameters for euroc

---
 .../mc_att_control/mc_att_control_sim.cpp     | 29 +++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index faf7294208..a8467f6b12 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -46,6 +46,35 @@
 using namespace std;
 #endif
 
+MulticopterAttitudeControlSim::MulticopterAttitudeControlSim()
+{
+	/* setup standard gains */
+	//XXX: make these configurable
+	_params.att_p(0)      = 5.0;
+	_params.rate_p(0)     = 0.05;
+	_params.rate_i(0)     = 0.0;
+	_params.rate_d(0)     = 0.003;
+	/* pitch gains */
+	_params.att_p(1)      = 5.0;
+	_params.rate_p(1)     = 0.05;
+	_params.rate_i(1)     = 0.0;
+	_params.rate_d(1)     = 0.003;
+	/* yaw gains */
+	_params.att_p(2)      = 2.8;
+	_params.rate_p(2)     = 0.2;
+	_params.rate_i(2)     = 0.1;
+	_params.rate_d(2)     = 0.0;
+	_params.yaw_rate_max  = 0.5;
+	_params.yaw_ff        = 0.5;
+	_params.man_roll_max  = 0.6;
+	_params.man_pitch_max = 0.6;
+	_params.man_yaw_max   = 0.6;
+}
+
+MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim()
+{
+}
+
 void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
 {
 	math::Quaternion quat;

From ebcad32e377824cb47186a400d22195cacd07e3b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:29:57 +0100
Subject: [PATCH 124/416] mc att: more cleanup between base and main class

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 10 +++++-----
 src/modules/mc_att_control/mc_att_control_base.h   | 10 +++++-----
 src/modules/mc_att_control/mc_att_control_main.cpp | 12 ++++++++++--
 3 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index a3b0340d2e..305fbaf892 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -32,6 +32,11 @@
 /**
  * @file mc_att_control_base.h
  *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -47,11 +52,6 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
-	_task_should_exit(false),
-	_control_task(-1),
-
-	_actuators_0_circuit_breaker_enabled(false),
-
 	_publish_att_sp(false)
 
 {
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index e7793e624c..662d32ee44 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -35,6 +35,11 @@
 /**
  * @file mc_att_control_base.h
  *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -88,11 +93,6 @@ public:
 	void set_actuator_controls();
 
 protected:
-
-	bool _task_should_exit; /**< if true, sensor task should exit */
-	int _control_task; /**< task handle for sensor task */
-	bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
-
 	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 7a03553f9c..16a3b627fd 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -38,6 +38,9 @@
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
  * The controller has two loops: P loop for angular error and PD loop for angular rate error.
  * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
@@ -110,8 +113,10 @@ public:
 	int		start();
 
 private:
-	bool	_task_should_exit;		/**< if true, sensor task should exit */
-	int		_control_task;			/**< task handle for sensor task */
+	bool		_task_should_exit;			/**< if true, sensor task should exit */
+	int		_control_task;				/**< task handle for sensor task */
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
 
 	int		_v_att_sub;				/**< vehicle attitude subscription */
 	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
@@ -211,6 +216,9 @@ MulticopterAttitudeControl	*g_control;
 
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	MulticopterAttitudeControlBase(),
+	_task_should_exit(false),
+	_control_task(-1),
+	_actuators_0_circuit_breaker_enabled(false),
 	/* subscriptions */
 	_v_att_sub(-1),
 	_v_att_sp_sub(-1),

From 0f1a7e7b5b699601886adec5215bf818b050a758 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:33:26 +0100
Subject: [PATCH 125/416] whitespace

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 4 ++--
 src/modules/mc_att_control/mc_att_control_base.h   | 4 ++--
 src/modules/mc_att_control/mc_att_control_sim.cpp  | 4 ++--
 src/modules/mc_att_control/mc_att_control_sim.h    | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 305fbaf892..17270884cc 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -20,8 +20,8 @@
 * 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
+* 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
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 662d32ee44..115667eb9f 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -23,8 +23,8 @@
 * 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
+* 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
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index a8467f6b12..788f92661b 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -20,8 +20,8 @@
 * 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
+* 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
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
index 0b48455af0..ecc065e68e 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.h
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -23,8 +23,8 @@
 * 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
+* 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

From c64c184948bed07f354904c0549b93225c85ad58 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 09:33:26 +0100
Subject: [PATCH 126/416] whitespace

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 4 ++--
 src/modules/mc_att_control/mc_att_control_base.h   | 4 ++--
 src/modules/mc_att_control/mc_att_control_sim.cpp  | 4 ++--
 src/modules/mc_att_control/mc_att_control_sim.h    | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 305fbaf892..17270884cc 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -20,8 +20,8 @@
 * 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
+* 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
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 662d32ee44..115667eb9f 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -23,8 +23,8 @@
 * 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
+* 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
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index a8467f6b12..788f92661b 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -20,8 +20,8 @@
 * 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
+* 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
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
index 0b48455af0..ecc065e68e 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.h
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -23,8 +23,8 @@
 * 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
+* 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

From 9c4fc14f230d4ebe2d82cf39ac5f4bd7f04d0c26 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 10:32:52 +0100
Subject: [PATCH 127/416] fix base and sim file descriptions

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 4 +++-
 src/modules/mc_att_control/mc_att_control_base.h   | 2 ++
 src/modules/mc_att_control/mc_att_control_sim.cpp  | 4 +++-
 src/modules/mc_att_control/mc_att_control_sim.h    | 4 +++-
 4 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 17270884cc..5e65e65087 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -30,7 +30,9 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_base.h
+ * @file mc_att_control_base.cpp
+ *
+ * MC Attitude Controller
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 115667eb9f..1c2f3fb6fe 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -34,6 +34,8 @@
 
 /**
  * @file mc_att_control_base.h
+ * 
+ * MC Attitude Controller
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index 788f92661b..d516d7e528 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -30,7 +30,9 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_base.h
+ * @file mc_att_control_sim.cpp
+ *
+ * MC Attitude Controller Interface for usage in a simulator
  *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
index ecc065e68e..a1bf44fc96 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.h
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -33,7 +33,9 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_base.h
+ * @file mc_att_control_sim.h
+ *
+ * MC Attitude Controller Interface for usage in a simulator
  *
  * @author Roman Bapst <bapstr@ethz.ch>
  *

From 996438aafedbc23e86ca36171dc81ec23d322caa Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 5 Dec 2014 10:32:52 +0100
Subject: [PATCH 128/416] fix base and sim file descriptions

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 4 +++-
 src/modules/mc_att_control/mc_att_control_base.h   | 2 ++
 src/modules/mc_att_control/mc_att_control_sim.cpp  | 4 +++-
 src/modules/mc_att_control/mc_att_control_sim.h    | 4 +++-
 4 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 17270884cc..5e65e65087 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -30,7 +30,9 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_base.h
+ * @file mc_att_control_base.cpp
+ *
+ * MC Attitude Controller
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 115667eb9f..1c2f3fb6fe 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -34,6 +34,8 @@
 
 /**
  * @file mc_att_control_base.h
+ * 
+ * MC Attitude Controller
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control/mc_att_control_sim.cpp
index 788f92661b..d516d7e528 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.cpp
+++ b/src/modules/mc_att_control/mc_att_control_sim.cpp
@@ -30,7 +30,9 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_base.h
+ * @file mc_att_control_sim.cpp
+ *
+ * MC Attitude Controller Interface for usage in a simulator
  *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control/mc_att_control_sim.h
index ecc065e68e..a1bf44fc96 100644
--- a/src/modules/mc_att_control/mc_att_control_sim.h
+++ b/src/modules/mc_att_control/mc_att_control_sim.h
@@ -33,7 +33,9 @@
 ****************************************************************************/
 
 /**
- * @file mc_att_control_base.h
+ * @file mc_att_control_sim.h
+ *
+ * MC Attitude Controller Interface for usage in a simulator
  *
  * @author Roman Bapst <bapstr@ethz.ch>
  *

From 52c35a8e20c99de4e7b6e27856d340a3a355250c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 6 Dec 2014 15:36:58 +0100
Subject: [PATCH 129/416] solve conflict for definiton of FILE

---
 NuttX                                  | 2 +-
 makefiles/toolchain_gnu-arm-eabi.mk    | 2 +-
 src/examples/subscriber/subscriber.cpp | 1 +
 3 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/NuttX b/NuttX
index ae4b05e2c5..dbe128ceaf 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c
+Subproject commit dbe128ceaf0e486b2ef799f14c13d38586e6cca8
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 84e5cd08a2..b98b1a0afe 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
 # Language-specific flags
 #
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
 
 # Generic warnings
 #
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 68003b6dfc..0d9ca1fa76 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -27,6 +27,7 @@
 
 #include <px4.h>
 #include "subscriber_params.h"
+#include <cstdio>
 
 using namespace px4;
 

From fd01c7fc58db1fca43f06b277cb2a0094440ecb3 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 6 Dec 2014 18:43:43 +0100
Subject: [PATCH 130/416] set NuttX submodule

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index dbe128ceaf..89a6776fc1 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit dbe128ceaf0e486b2ef799f14c13d38586e6cca8
+Subproject commit 89a6776fc1b31d242c637d3f19072609bb98ec6c

From 65629d09d5e2424c6fcaf261c95f6d6995c4afd7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 10:35:59 +0100
Subject: [PATCH 131/416] add mc att to cmakelist

---
 CMakeLists.txt | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 636cc07e34..e91f2cc371 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -136,6 +136,16 @@ target_link_libraries(subscriber
   px4
 )
 
+add_executable(mc_att_control
+	src/modules/mc_att_control/mc_att_control_main.cpp
+	src/moudles/mc_att_control/mc_att_control_base.cpp)
+add_dependencies(mc_att_control px4_generate_messages_cpp)
+target_link_libraries(mc_att_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
+
 #############
 ## Install ##
 #############

From 87df7c3243412d4fc7a0c40967b2abe078f7a0b2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 10:37:01 +0100
Subject: [PATCH 132/416] move vehicle_attitude_setpoint to msg format

---
 msg/px4_msgs/vehicle_attitude_setpoint.msg    | 21 +++++++
 .../fw_att_control/fw_att_control_base.cpp    |  3 +-
 src/modules/mavlink/mavlink_receiver.cpp      |  2 +-
 .../mc_att_control/mc_att_control_base.cpp    |  4 +-
 .../mc_att_control/mc_att_control_base.h      |  7 +--
 .../mc_att_control/mc_att_control_main.cpp    |  6 +-
 .../mc_pos_control/mc_pos_control_main.cpp    | 22 +++----
 .../uORB/topics/vehicle_attitude_setpoint.h   | 59 +++++++------------
 src/platforms/px4_defines.h                   |  1 +
 src/platforms/px4_includes.h                  |  1 +
 src/platforms/px4_middleware.h                |  4 +-
 11 files changed, 66 insertions(+), 64 deletions(-)
 create mode 100644 msg/px4_msgs/vehicle_attitude_setpoint.msg

diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/px4_msgs/vehicle_attitude_setpoint.msg
new file mode 100644
index 0000000000..1a8e6e3d54
--- /dev/null
+++ b/msg/px4_msgs/vehicle_attitude_setpoint.msg
@@ -0,0 +1,21 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body					# body angle in NED frame
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
index 46fef3e67b..f543c02f9c 100644
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ b/src/modules/fw_att_control/fw_att_control_base.cpp
@@ -31,7 +31,7 @@
 
 /**
  * @file mc_att_control_base.cpp
- * 
+ *
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
@@ -40,7 +40,6 @@
 #include <math.h>
 #include <mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
-#include <systemlib/err.h>
 
 using namespace std;
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index e98d72afe7..cb55a25aa7 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
-					mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
+					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
 					att_sp.R_valid = true;
 					att_sp.thrust = set_attitude_target.thrust;
 					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 5e65e65087..f6b3268b59 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -172,7 +172,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	if (_v_att_sp.R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0][0]);
+		R_sp.set(&_v_att_sp.R_body[0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
@@ -180,7 +180,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 				_v_att_sp.yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
+		memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0],
 		       sizeof(_v_att_sp.R_body));
 		_v_att_sp.R_valid = true;
 	}
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 1c2f3fb6fe..c1ea52f636 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -34,7 +34,7 @@
 
 /**
  * @file mc_att_control_base.h
- * 
+ *
  * MC Attitude Controller
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -45,23 +45,20 @@
  * @author Roman Bapst <bapstr@ethz.ch>
  *
  */
-
+#include <px4.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
 #include <errno.h>
 #include <math.h>
-#include <drivers/drv_hrt.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/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
-#include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <lib/mathlib/mathlib.h>
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 887a535088..c0e8957e0f 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -52,8 +52,7 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
-#include <nuttx/config.h>
-#include <stdio.h>
+#include <px4.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -62,8 +61,6 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
-#include <uORB/uORB.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>
@@ -78,6 +75,7 @@
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
+
 #include "mc_att_control_base.h"
 
 /**
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index cea847603e..4ea5fdfb64 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -43,8 +43,9 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <nuttx/config.h>
-#include <stdio.h>
+#include <px4.h>
+#include <functional>
+#include <cstdio>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -54,8 +55,7 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
-#include <uORB/uORB.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>
@@ -67,8 +67,8 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
+// #include <systemlib/param/param.h>
+// #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
@@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp()
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
 		/* shift position setpoint to make attitude setpoint continuous */
-		_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
-		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
@@ -987,7 +987,7 @@ MulticopterPositionControl::task_main()
 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
-				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 				_att_sp.R_valid = true;
 
 				_att_sp.roll_body = 0.0f;
@@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main()
 						}
 
 						/* copy rotation matrix to attitude setpoint topic */
-						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
 						/* calculate euler angles, for logging only, must not be used for control */
@@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main()
 						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
 
 						/* copy rotation matrix to attitude setpoint topic */
-						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
 						_att_sp.roll_body = 0.0f;
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index a503aa0c69..29fb104dfb 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,49 +31,37 @@
  *
  ****************************************************************************/
 
-/**
- * @file vehicle_attitude_setpoint.h
- * Definition of the vehicle attitude setpoint uORB topic.
- */
+ /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */
 
-#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
+
+#pragma once
 
 #include <stdint.h>
-#include <stdbool.h>
-#include <platforms/px4_defines.h>
+#include "../uORB.h"
+
+
 
 /**
  * @addtogroup topics
  * @{
  */
 
-/**
- * vehicle attitude setpoint.
- */
+
 struct vehicle_attitude_setpoint_s {
-	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	float roll_body;				/**< body angle in NED frame		*/
-	float pitch_body;				/**< body angle in NED frame		*/
-	float yaw_body;					/**< body angle in NED frame		*/
-	//float body_valid;				/**< Set to true if body angles are valid */
-
-	float R_body[3][3];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
-	bool R_valid;					/**< Set to true if rotation matrix is valid */
-
-	//! For quaternion-based attitude control
-	float q_d[4];				/** Desired quaternion for quaternion control */
-	bool q_d_valid;					/**< Set to true if quaternion vector is valid */
-	float q_e[4];				/** Attitude error in quaternion */
-	bool q_e_valid;					/**< Set to true if quaternion error vector is valid */
-
-	float thrust;					/**< Thrust in Newton the power system should generate */
-
-	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
-	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
-	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
-
+	uint64_t timestamp;
+	float roll_body;
+	float pitch_body;
+	float yaw_body;
+	float R_body[9];
+	bool R_valid;
+	float q_d[4];
+	bool q_d_valid;
+	float q_e[4];
+	bool q_e_valid;
+	float thrust;
+	bool roll_reset_integral;
+	bool pitch_reset_integral;
+	bool yaw_reset_integral;
 };
 
 /**
@@ -83,5 +70,3 @@ struct vehicle_attitude_setpoint_s {
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_attitude_setpoint);
-
-#endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index af7188f32c..ff302448a5 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -115,6 +115,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
 
 /* Parameter handle datatype */
+#include <systemlib/param/param.h>
 typedef param_t px4_param_t;
 
 /* Initialize a param, get param handle */
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index a3b59b766e..a9425077eb 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -55,6 +55,7 @@
 #include <nuttx/config.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index c1465b4b1b..b0bc404173 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -53,13 +53,13 @@ __EXPORT uint64_t get_time_micros();
 /**
  * Returns true if the app/task should continue to run
  */
-bool ok() { return ros::ok(); }
+inline bool ok() { return ros::ok(); }
 #else
 extern bool task_should_exit;
 /**
  * Returns true if the app/task should continue to run
  */
-bool ok() { return !task_should_exit; }
+inline bool ok() { return !task_should_exit; }
 #endif
 
 class Rate

From b3600e5ee6fc4550533d65c1c25abceb6db4466e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 11:06:56 +0100
Subject: [PATCH 133/416] manual_control_setpoint as msg

---
 msg/px4_msgs/manual_control_setpoint.msg      | 44 +++++++++
 .../mc_att_control/mc_att_control_base.h      |  1 -
 .../mc_att_control/mc_att_control_main.cpp    |  1 -
 src/modules/sensors/sensors.cpp               |  8 +-
 .../uORB/topics/manual_control_setpoint.h     | 98 +++++++------------
 src/platforms/px4_includes.h                  |  1 +
 6 files changed, 82 insertions(+), 71 deletions(-)
 create mode 100644 msg/px4_msgs/manual_control_setpoint.msg

diff --git a/msg/px4_msgs/manual_control_setpoint.msg b/msg/px4_msgs/manual_control_setpoint.msg
new file mode 100644
index 0000000000..d3cfb078be
--- /dev/null
+++ b/msg/px4_msgs/manual_control_setpoint.msg
@@ -0,0 +1,44 @@
+
+uint8 SWITCH_POS_NONE = 0		# switch is not mapped
+uint8 SWITCH_POS_ON = 1			# switch activated (value = 1)
+uint8 SWITCH_POS_MIDDLE = 2		# middle position (value = 0)
+uint8 SWITCH_POS_OFF = 3		# switch not activated (value = -1)
+uint64 timestamp
+
+# Any of the channels may not be available and be set to NaN
+# to indicate that it does not contain valid data.
+# The variable names follow the definition of the
+# MANUAL_CONTROL mavlink message.
+# The default range is from -1 to 1 (mavlink message -1000 to 1000)
+# The range for the z variable is defined from 0 to 1. (The z field of
+# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
+
+float32 x			 # stick position in x direction -1..1
+				 # in general corresponds to forward/back motion or pitch of vehicle,
+				 # in general a positive value means forward or negative pitch and
+				 # a negative value means backward or positive pitch
+float32 y			 # stick position in y direction -1..1
+				 # in general corresponds to right/left motion or roll of vehicle,
+				 # in general a positive value means right or positive roll and
+				 # a negative value means left or negative roll
+float32 z			 # throttle stick position 0..1
+				 # in general corresponds to up/down motion or thrust of vehicle,
+				 # in general the value corresponds to the demanded throttle by the user,
+				 # if the input is used for setting the setpoint of a vertical position
+				 # controller any value > 0.5 means up and any value < 0.5 means down
+float32 r			 # yaw stick/twist positon, -1..1
+				 # in general corresponds to the righthand rotation around the vertical
+				 # (downwards) axis of the vehicle
+float32 flaps			 # flap position
+float32 aux1			 # default function: camera yaw / azimuth
+float32 aux2			 # default function: camera pitch / tilt
+float32 aux3			 # default function: camera trigger
+float32 aux4			 # default function: camera roll
+float32 aux5			 # default function: payload drop
+
+uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
+uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
+uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
+uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
+uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
+uint8 offboard_switch		 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index c1ea52f636..d42d672c00 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,7 +53,6 @@
 #include <errno.h>
 #include <math.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>
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index c0e8957e0f..a8a3ae6b0b 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -61,7 +61,6 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.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>
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 672dc52c3d..fca72c3040 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -187,8 +187,8 @@ private:
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
-	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
+	uint8_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	uint8_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
 
 	/**
 	 * Gather and publish RC input data.
@@ -1512,7 +1512,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
 	}
 }
 
-switch_pos_t
+uint8_t
 Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
@@ -1533,7 +1533,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi
 	}
 }
 
-switch_pos_t
+uint8_t
 Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 {
 	if (_rc.function[func] >= 0) {
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 13138ebc9a..440841f903 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -1,21 +1,20 @@
 /****************************************************************************
  *
- *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  *
  * 1. Redistributions of source code must retain the above copyright
- *		notice, this list of conditions and the following disclaimer.
+ *    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.
+ *    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.
+ *    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
@@ -32,73 +31,44 @@
  *
  ****************************************************************************/
 
-/**
- * @file manual_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
+ /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/manual_control_setpoint.msg */
 
-#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
-#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
+
+#pragma once
 
 #include <stdint.h>
-#include <platforms/px4_defines.h>
+#include "../uORB.h"
+
+#define SWITCH_POS_NONE 0
+#define SWITCH_POS_ON 1
+#define SWITCH_POS_MIDDLE 2
+#define SWITCH_POS_OFF 3
 
-/**
- * Switch position
- */
-typedef enum {
-	SWITCH_POS_NONE = 0,		/**< switch is not mapped */
-	SWITCH_POS_ON,			/**< switch activated (value = 1) */
-	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
-	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
-} switch_pos_t;
 
 /**
  * @addtogroup topics
  * @{
  */
 
+
 struct manual_control_setpoint_s {
 	uint64_t timestamp;
-
-	/**
-	 * Any of the channels may not be available and be set to NaN
-	 * to indicate that it does not contain valid data.
-	 * The variable names follow the definition of the
-	 * MANUAL_CONTROL mavlink message.
-	 * The default range is from -1 to 1 (mavlink message -1000 to 1000)
-	 * The range for the z variable is defined from 0 to 1. (The z field of
-	 * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
-	 */
-	float x;			/**< stick position in x direction -1..1
-					  in general corresponds to forward/back motion or pitch of vehicle,
-					  in general a positive value means forward or negative pitch and
-					  a negative value means backward or positive pitch */
-	float y;			/**< stick position in y direction -1..1
-					  in general corresponds to right/left motion or roll of vehicle,
-					  in general a positive value means right or positive roll and
-					  a negative value means left or negative roll */
-	float z;			/**< throttle stick position 0..1
-					  in general corresponds to up/down motion or thrust of vehicle,
-					  in general the value corresponds to the demanded throttle by the user,
-					  if the input is used for setting the setpoint of a vertical position
-					  controller any value > 0.5 means up and any value < 0.5 means down */
-	float r;			/**< yaw stick/twist positon, -1..1
-					  in general corresponds to the righthand rotation around the vertical
-					  (downwards) axis of the vehicle */
-	float flaps;			/**< flap position */
-	float aux1;			/**< default function: camera yaw / azimuth */
-	float aux2;			/**< default function: camera pitch / tilt */
-	float aux3;			/**< default function: camera trigger */
-	float aux4;			/**< default function: camera roll */
-	float aux5;			/**< default function: payload drop */
-
-	switch_pos_t mode_switch;	/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
-	switch_pos_t return_switch;	/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
-	switch_pos_t posctl_switch;	/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
-	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
-	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
-	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
+	float x;
+	float y;
+	float z;
+	float r;
+	float flaps;
+	float aux1;
+	float aux2;
+	float aux3;
+	float aux4;
+	float aux5;
+	uint8_t mode_switch;
+	uint8_t return_switch;
+	uint8_t posctl_switch;
+	uint8_t loiter_switch;
+	uint8_t acro_switch;
+	uint8_t offboard_switch;
 };
 
 /**
@@ -107,5 +77,3 @@ struct manual_control_setpoint_s {
 
 /* register this as object request broker structure */
 ORB_DECLARE(manual_control_setpoint);
-
-#endif
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index a9425077eb..2045bd2a85 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -56,6 +56,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 

From dc945a3f3f9f7295c73fc8280d6a0048e059c469 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 12:12:23 +0100
Subject: [PATCH 134/416] actuator controls as msg

---
 msg/px4_msgs/actuator_controls.msg            |  4 ++
 msg/templates/msg.h.template                  |  3 +-
 .../mc_att_control/mc_att_control_base.h      |  2 +-
 .../mc_att_control/mc_att_control_main.cpp    |  6 ---
 src/modules/uORB/topics/actuator_controls.h   | 38 ++++++-------------
 src/platforms/px4_defines.h                   |  7 ++++
 src/platforms/px4_includes.h                  |  4 ++
 src/systemcmds/esc_calib/esc_calib.c          |  1 +
 8 files changed, 30 insertions(+), 35 deletions(-)
 create mode 100644 msg/px4_msgs/actuator_controls.msg

diff --git a/msg/px4_msgs/actuator_controls.msg b/msg/px4_msgs/actuator_controls.msg
new file mode 100644
index 0000000000..743f20cdff
--- /dev/null
+++ b/msg/px4_msgs/actuator_controls.msg
@@ -0,0 +1,4 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+float32[8] control
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index 5fce506b6b..84dea37a3b 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t',
   'uint32': 'uint32_t',
   'uint64': 'uint64_t',
   'float32': 'float',
-  'bool': 'bool'}
+  'bool': 'bool',
+  'actuator_controls': 'actuator_controls'}
 
 # Function to print a standard ros type
 def print_field_def(field):
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index d42d672c00..c7853a0ecb 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,11 +53,11 @@
 #include <errno.h>
 #include <math.h>
 
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
 #include <systemlib/perf_counter.h>
 #include <lib/mathlib/mathlib.h>
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a8a3ae6b0b..6ab4f95cd1 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -61,12 +61,6 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 43f7a59ee9..3e43107adf 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,48 +31,32 @@
  *
  ****************************************************************************/
 
-/**
- * @file actuator_controls.h
- *
- * Actuator control topics - mixer inputs.
- *
- * Values published to these topics are the outputs of the vehicle control
- * system, and are expected to be mixed and used to drive the actuators
- * (servos, speed controls, etc.) that operate the vehicle.
- *
- * Each topic can be published by a single controller
- */
+ /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */
 
-#ifndef TOPIC_ACTUATOR_CONTROLS_H
-#define TOPIC_ACTUATOR_CONTROLS_H
+
+#pragma once
 
 #include <stdint.h>
-#include <platforms/px4_defines.h>
+#include "../uORB.h"
 
-#define NUM_ACTUATOR_CONTROLS		8
-#define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
+#define NUM_ACTUATOR_CONTROLS 8
+#define NUM_ACTUATOR_CONTROL_GROUPS 4
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
 
 /**
  * @addtogroup topics
  * @{
  */
 
+
 struct actuator_controls_s {
 	uint64_t timestamp;
-	float	control[NUM_ACTUATOR_CONTROLS];
+	float control[8];
 };
 
 /**
  * @}
  */
 
-/* actuator control sets; this list can be expanded as more controllers emerge */
-ORB_DECLARE(actuator_controls_0);
-ORB_DECLARE(actuator_controls_1);
-ORB_DECLARE(actuator_controls_2);
-ORB_DECLARE(actuator_controls_3);
-
-#endif
+/* register this as object request broker structure */
+ORB_DECLARE(actuator_controls);
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index ff302448a5..daeff9cf30 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -139,3 +139,10 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
+
+/* Diverese uORB header defiens */ //XXX: move to better location
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
+ORB_DECLARE(actuator_controls_0);
+ORB_DECLARE(actuator_controls_1);
+ORB_DECLARE(actuator_controls_2);
+ORB_DECLARE(actuator_controls_3);
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 2045bd2a85..3e94586c14 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -57,6 +57,10 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 7d80af3079..20967b541d 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -39,6 +39,7 @@
  */
 
 #include <nuttx/config.h>
+#include <platforms/px4_defines.h>
 
 #include <stdio.h>
 #include <stdlib.h>

From fc4b722e729fda1ed45aa214360b5bb9b462a2b9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 12:17:30 +0100
Subject: [PATCH 135/416] fix headers, remove unneded uorb headers

---
 .../mc_att_control/mc_att_control_base.h      |  1 -
 src/modules/uORB/topics/actuator_controls.h   | 62 ---------------
 .../uORB/topics/manual_control_setpoint.h     | 79 -------------------
 .../uORB/topics/vehicle_attitude_setpoint.h   | 72 -----------------
 src/platforms/px4_includes.h                  |  6 +-
 5 files changed, 2 insertions(+), 218 deletions(-)
 delete mode 100644 src/modules/uORB/topics/actuator_controls.h
 delete mode 100644 src/modules/uORB/topics/manual_control_setpoint.h
 delete mode 100644 src/modules/uORB/topics/vehicle_attitude_setpoint.h

diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index c7853a0ecb..5e9c0c4db6 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,7 +53,6 @@
 #include <errno.h>
 #include <math.h>
 
-#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
deleted file mode 100644
index 3e43107adf..0000000000
--- a/src/modules/uORB/topics/actuator_controls.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
- /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_controls.msg */
-
-
-#pragma once
-
-#include <stdint.h>
-#include "../uORB.h"
-
-#define NUM_ACTUATOR_CONTROLS 8
-#define NUM_ACTUATOR_CONTROL_GROUPS 4
-
-
-/**
- * @addtogroup topics
- * @{
- */
-
-
-struct actuator_controls_s {
-	uint64_t timestamp;
-	float control[8];
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(actuator_controls);
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
deleted file mode 100644
index 440841f903..0000000000
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
- /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/manual_control_setpoint.msg */
-
-
-#pragma once
-
-#include <stdint.h>
-#include "../uORB.h"
-
-#define SWITCH_POS_NONE 0
-#define SWITCH_POS_ON 1
-#define SWITCH_POS_MIDDLE 2
-#define SWITCH_POS_OFF 3
-
-
-/**
- * @addtogroup topics
- * @{
- */
-
-
-struct manual_control_setpoint_s {
-	uint64_t timestamp;
-	float x;
-	float y;
-	float z;
-	float r;
-	float flaps;
-	float aux1;
-	float aux2;
-	float aux3;
-	float aux4;
-	float aux5;
-	uint8_t mode_switch;
-	uint8_t return_switch;
-	uint8_t posctl_switch;
-	uint8_t loiter_switch;
-	uint8_t acro_switch;
-	uint8_t offboard_switch;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(manual_control_setpoint);
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
deleted file mode 100644
index 29fb104dfb..0000000000
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
- /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */
-
-
-#pragma once
-
-#include <stdint.h>
-#include "../uORB.h"
-
-
-
-/**
- * @addtogroup topics
- * @{
- */
-
-
-struct vehicle_attitude_setpoint_s {
-	uint64_t timestamp;
-	float roll_body;
-	float pitch_body;
-	float yaw_body;
-	float R_body[9];
-	bool R_valid;
-	float q_d[4];
-	bool q_d_valid;
-	float q_e[4];
-	bool q_e_valid;
-	float thrust;
-	bool roll_reset_integral;
-	bool pitch_reset_integral;
-	bool yaw_reset_integral;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude_setpoint);
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 3e94586c14..8b0c374029 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -57,10 +57,8 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 

From 99d89577cd4c7e663583773eccf0883c1df773c4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 12:23:27 +0100
Subject: [PATCH 136/416] vehicle rates sp as msg

---
 msg/px4_msgs/vehicle_rates_setpoint.msg       |  6 ++
 .../uORB/topics/vehicle_rates_setpoint.h      | 67 -------------------
 2 files changed, 6 insertions(+), 67 deletions(-)
 create mode 100644 msg/px4_msgs/vehicle_rates_setpoint.msg
 delete mode 100644 src/modules/uORB/topics/vehicle_rates_setpoint.h

diff --git a/msg/px4_msgs/vehicle_rates_setpoint.msg b/msg/px4_msgs/vehicle_rates_setpoint.msg
new file mode 100644
index 0000000000..834113c798
--- /dev/null
+++ b/msg/px4_msgs/vehicle_rates_setpoint.msg
@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
deleted file mode 100644
index e5cecf02b4..0000000000
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 vehicle_rates_setpoint.h
- * Definition of the vehicle rates setpoint topic
- */
-
-#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
-#define TOPIC_VEHICLE_RATES_SETPOINT_H_
-
-#include <stdint.h>
-#include <platforms/px4_defines.h>
-
-/**
- * @addtogroup topics
- * @{
- */
-struct vehicle_rates_setpoint_s {
-	uint64_t timestamp; /**< in microseconds since system start */
-
-	float roll;	/**< body angular rates in NED frame		*/
-	float pitch;	/**< body angular rates in NED frame		*/
-	float yaw;	/**< body angular rates in NED frame		*/
-	float thrust;	/**< thrust normalized to 0..1			*/
-
-}; /**< vehicle_rates_setpoint */
-
-/**
-* @}
-*/
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_rates_setpoint);
-
-#endif

From a71e6ed3c64d5c1c94d6f5f445b4b746ccf37eff Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 12:25:39 +0100
Subject: [PATCH 137/416] change headers to use vehicle attitude msg

---
 src/modules/mc_att_control/mc_att_control_base.h | 1 -
 src/platforms/px4_includes.h                     | 1 +
 2 files changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 5e9c0c4db6..6c4b5c707d 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,7 +53,6 @@
 #include <errno.h>
 #include <math.h>
 
-#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 8b0c374029..950f5cc795 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -59,6 +59,7 @@
 #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 <systemlib/err.h>
 #include <systemlib/param/param.h>
 

From b93fcca4339edc5312158dc4a70bf8abf2c8bb4b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 12:34:57 +0100
Subject: [PATCH 138/416] vehicle cotnrol mode as msg

---
 msg/px4_msgs/vehicle_control_mode.msg         | 22 +++++
 .../fw_pos_control_l1_main.cpp                |  1 +
 .../mc_att_control/mc_att_control_base.h      |  1 -
 .../uORB/topics/vehicle_control_mode.h        | 95 -------------------
 src/platforms/px4_includes.h                  |  1 +
 5 files changed, 24 insertions(+), 96 deletions(-)
 create mode 100644 msg/px4_msgs/vehicle_control_mode.msg
 delete mode 100644 src/modules/uORB/topics/vehicle_control_mode.h

diff --git a/msg/px4_msgs/vehicle_control_mode.msg b/msg/px4_msgs/vehicle_control_mode.msg
new file mode 100644
index 0000000000..153a642bb4
--- /dev/null
+++ b/msg/px4_msgs/vehicle_control_mode.msg
@@ -0,0 +1,22 @@
+
+uint64 timestamp				# in microseconds since system start
+						# is set whenever the writing thread stores new data
+
+bool flag_armed
+
+bool flag_external_manual_override_ok		# external override non-fatal for system. Only true for fixed wing
+
+# XXX needs yet to be set by state machine helper
+bool flag_system_hil_enabled
+
+bool flag_control_manual_enabled		# true if manual input is mixed in
+bool flag_control_auto_enabled			# true if onboard autopilot should act
+bool flag_control_offboard_enabled		# true if offboard control should be used
+bool flag_control_rates_enabled			# true if rates are stabilized
+bool flag_control_attitude_enabled		# true if attitude stabilization is mixed in
+bool flag_control_force_enabled			# true if force control is mixed in
+bool flag_control_velocity_enabled		# true if horizontal velocity (implies direction) is controlled
+bool flag_control_position_enabled		# true if position is controlled
+bool flag_control_altitude_enabled		# true if altitude is controlled
+bool flag_control_climb_rate_enabled		# true if climb rate is controlled
+bool flag_control_termination_enabled		# true if flighttermination is enabled
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index e07bcc225f..f8399d10be 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 6c4b5c707d..036dd4c48f 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,7 +53,6 @@
 #include <errno.h>
 #include <math.h>
 
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/perf_counter.h>
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
deleted file mode 100644
index 2dd8550bc4..0000000000
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_control_mode.h
- * Definition of the vehicle_control_mode uORB topic.
- *
- * All control apps should depend their actions based on the flags set here.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef VEHICLE_CONTROL_MODE
-#define VEHICLE_CONTROL_MODE
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <platforms/px4_defines.h>
-#include "vehicle_status.h"
-
-/**
- * @addtogroup topics @{
- */
-
-
-/**
- * state machine / state of vehicle.
- *
- * Encodes the complete system state and is set by the commander app.
- */
-
-struct vehicle_control_mode_s {
-	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	bool flag_armed;
-
-	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
-
-	// XXX needs yet to be set by state machine helper
-	bool flag_system_hil_enabled;
-
-	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
-	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
-	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
-	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_force_enabled;		/**< true if force control is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
-	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_control_mode);
-
-#endif
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 950f5cc795..a8b94076e6 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -60,6 +60,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 

From 356e6f1eebfba8bc8630299ef091570f0c337ba5 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 13:42:40 +0100
Subject: [PATCH 139/416] WIP, move some ORB defines

---
 src/modules/uORB/uORB.h     | 9 ++++++++-
 src/platforms/px4_defines.h | 7 -------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 82ff46ad26..578ebf9ddc 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -125,7 +125,7 @@ typedef intptr_t	orb_advert_t;
  * node in /obj if required and publishes the initial data.
  *
  * Any number of advertisers may publish to a topic; publications are atomic
- * but co-ordination between publishers is not provided by the ORB. 
+ * but co-ordination between publishers is not provided by the ORB.
  *
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
@@ -261,4 +261,11 @@ extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
 
 __END_DECLS
 
+/* Diverse uORB header defines */ //XXX: move to better location
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
+ORB_DECLARE(actuator_controls_0);
+ORB_DECLARE(actuator_controls_1);
+ORB_DECLARE(actuator_controls_2);
+ORB_DECLARE(actuator_controls_3);
+
 #endif /* _UORB_UORB_H */
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index daeff9cf30..ff302448a5 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -139,10 +139,3 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
-
-/* Diverese uORB header defiens */ //XXX: move to better location
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
-ORB_DECLARE(actuator_controls_0);
-ORB_DECLARE(actuator_controls_1);
-ORB_DECLARE(actuator_controls_2);
-ORB_DECLARE(actuator_controls_3);

From 88f4931fd1ac301c18921a5c12884263ac6fd68c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 13:44:08 +0100
Subject: [PATCH 140/416] actuator armed as msg

---
 msg/px4_msgs/actuator_armed.msg               |  6 ++++
 msg/px4_msgs/unused/actuator_armed.msg        |  6 ----
 .../mc_att_control/mc_att_control_base.h      |  1 -
 src/modules/uORB/topics/actuator_armed.h      | 32 ++++++++-----------
 src/platforms/px4_includes.h                  |  1 +
 5 files changed, 20 insertions(+), 26 deletions(-)
 create mode 100644 msg/px4_msgs/actuator_armed.msg
 delete mode 100644 msg/px4_msgs/unused/actuator_armed.msg

diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/px4_msgs/actuator_armed.msg
new file mode 100644
index 0000000000..b83adb8f24
--- /dev/null
+++ b/msg/px4_msgs/actuator_armed.msg
@@ -0,0 +1,6 @@
+
+uint64 timestamp	# Microseconds since system boot
+bool armed		# Set to true if system is armed
+bool ready_to_arm	# Set to true if system is ready to be armed
+bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
+bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
diff --git a/msg/px4_msgs/unused/actuator_armed.msg b/msg/px4_msgs/unused/actuator_armed.msg
deleted file mode 100644
index f6bf583073..0000000000
--- a/msg/px4_msgs/unused/actuator_armed.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-
-uint64 timestamp	# Microseconds since system boot
-bool armed          # Set to true if system is armed
-bool ready_to_arm	# Set to true if system is ready to be armed
-bool lockdown	    # Set to true if actuators are forced to being disabled (due to emergency or HIL)
-bool force_failsafe # Set to true if the actuators are forced to the failsafe position
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 036dd4c48f..49c8dad7d3 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,7 +53,6 @@
 #include <errno.h>
 #include <math.h>
 
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/perf_counter.h>
 #include <lib/mathlib/mathlib.h>
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 1e10e0ad12..918a305fdd 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,32 +31,28 @@
  *
  ****************************************************************************/
 
-/**
- * @file actuator_armed.h
- *
- * Actuator armed topic
- *
- */
+ /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_armed.msg */
 
-#ifndef TOPIC_ACTUATOR_ARMED_H
-#define TOPIC_ACTUATOR_ARMED_H
+
+#pragma once
 
 #include <stdint.h>
-#include <platforms/px4_defines.h>
+#include "../uORB.h"
+
+
 
 /**
  * @addtogroup topics
  * @{
  */
 
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
 
-	uint64_t	timestamp;	/**< Microseconds since system boot */
-	bool		armed;		/**< Set to true if system is armed */
-	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-	bool		force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
+struct actuator_armed_s {
+	uint64_t timestamp;
+	bool armed;
+	bool ready_to_arm;
+	bool lockdown;
+	bool force_failsafe;
 };
 
 /**
@@ -65,5 +61,3 @@ struct actuator_armed_s {
 
 /* register this as object request broker structure */
 ORB_DECLARE(actuator_armed);
-
-#endif
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index a8b94076e6..b7771072ff 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -61,6 +61,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 

From 77fd7b388b3b80881e405bec5ed56fb959082612 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 14:04:20 +0100
Subject: [PATCH 141/416] parameter update as msg

---
 msg/px4_msgs/parameter_update.msg             |  1 +
 .../mc_att_control/mc_att_control_base.h      |  1 -
 src/modules/uORB/topics/parameter_update.h    | 61 -------------------
 src/platforms/px4_includes.h                  |  1 +
 4 files changed, 2 insertions(+), 62 deletions(-)
 create mode 100644 msg/px4_msgs/parameter_update.msg
 delete mode 100644 src/modules/uORB/topics/parameter_update.h

diff --git a/msg/px4_msgs/parameter_update.msg b/msg/px4_msgs/parameter_update.msg
new file mode 100644
index 0000000000..39dc336e05
--- /dev/null
+++ b/msg/px4_msgs/parameter_update.msg
@@ -0,0 +1 @@
+uint64 timestamp	# time at which the latest parameter was updated
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 49c8dad7d3..f81a0d7721 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -53,7 +53,6 @@
 #include <errno.h>
 #include <math.h>
 
-#include <uORB/topics/parameter_update.h>
 #include <systemlib/perf_counter.h>
 #include <lib/mathlib/mathlib.h>
 
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
deleted file mode 100644
index fe9c9070fc..0000000000
--- a/src/modules/uORB/topics/parameter_update.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file parameter_update.h
- * Notification about a parameter update.
- */
-
-#ifndef TOPIC_PARAMETER_UPDATE_H
-#define TOPIC_PARAMETER_UPDATE_H
-
-#include <stdint.h>
-#include <platforms/px4_defines.h>
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct parameter_update_s {
-	/** time at which the latest parameter was updated */
-	uint64_t	timestamp;
-};
-
-/**
- * @}
- */
-
-ORB_DECLARE(parameter_update);
-
-#endif
\ No newline at end of file
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index b7771072ff..b8c8d034ed 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -62,6 +62,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 

From 377030bd8a25f501c47aba573bf3125ad9061bd0 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 14:20:11 +0100
Subject: [PATCH 142/416] mc att ctl: remove code which is already in base
 class

---
 .../mc_att_control/mc_att_control_main.cpp    | 27 -------------------
 1 file changed, 27 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 6ab4f95cd1..9dec6b8e6b 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -227,33 +227,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
-	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_actuators, 0, sizeof(_actuators));
-	memset(&_armed, 0, sizeof(_armed));
-
-	_params.att_p.zero();
-	_params.rate_p.zero();
-	_params.rate_i.zero();
-	_params.rate_d.zero();
-	_params.yaw_ff = 0.0f;
-	_params.yaw_rate_max = 0.0f;
-	_params.man_roll_max = 0.0f;
-	_params.man_pitch_max = 0.0f;
-	_params.man_yaw_max = 0.0f;
-	_params.acro_rate_max.zero();
-
-	_rates_prev.zero();
-	_rates_sp.zero();
-	_rates_int.zero();
-	_thrust_sp = 0.0f;
-	_att_control.zero();
-
-	_I.identity();
-
 	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
 	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
 	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");

From b500cfb1f11b2000ad58d8c8b5b0d19ab9ebb438 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 14:20:11 +0100
Subject: [PATCH 143/416] mc att ctl: remove code which is already in base
 class

---
 .../mc_att_control/mc_att_control_main.cpp    | 27 -------------------
 1 file changed, 27 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 887a535088..a5226eb7c5 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -236,33 +236,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
-	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_actuators, 0, sizeof(_actuators));
-	memset(&_armed, 0, sizeof(_armed));
-
-	_params.att_p.zero();
-	_params.rate_p.zero();
-	_params.rate_i.zero();
-	_params.rate_d.zero();
-	_params.yaw_ff = 0.0f;
-	_params.yaw_rate_max = 0.0f;
-	_params.man_roll_max = 0.0f;
-	_params.man_pitch_max = 0.0f;
-	_params.man_yaw_max = 0.0f;
-	_params.acro_rate_max.zero();
-
-	_rates_prev.zero();
-	_rates_sp.zero();
-	_rates_int.zero();
-	_thrust_sp = 0.0f;
-	_att_control.zero();
-
-	_I.identity();
-
 	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
 	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
 	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");

From fe6663ad9aa0a5fdb18e2ce3e23dd57d47ae8569 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 15:09:31 +0100
Subject: [PATCH 144/416] add platforms/nuttx to default makefile

---
 src/modules/mc_att_control/mc_att_control_main.cpp | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 9dec6b8e6b..631ca57e05 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -103,6 +103,8 @@ public:
 	 */
 	int		start();
 
+	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+
 private:
 	bool		_task_should_exit;			/**< if true, sensor task should exit */
 	int		_control_task;				/**< task handle for sensor task */
@@ -422,11 +424,13 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
+	px4::NodeHandle n;
 
 	/*
 	 * do subscriptions
 	 */
 	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	// PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, MulticopterAttitudeControl::handle_vehicle_attitude, this 0);
 	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -582,6 +586,10 @@ MulticopterAttitudeControl::task_main()
 	_exit(0);
 }
 
+void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
+		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
+}
+
 int
 MulticopterAttitudeControl::start()
 {
@@ -603,8 +611,10 @@ MulticopterAttitudeControl::start()
 	return OK;
 }
 
-int mc_att_control_main(int argc, char *argv[])
+PX4_MAIN_FUNCTION(mc_att_control)
 {
+	px4::init(argc, argv, "mc_att_control");
+
 	if (argc < 1) {
 		errx(1, "usage: mc_att_control {start|stop|status}");
 	}

From afa835744cbef667279913bbf79a0c4697a5ec8d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 15:48:44 +0100
Subject: [PATCH 145/416] remove unnecessary type in msg.h template

---
 msg/templates/msg.h.template | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index 84dea37a3b..5fce506b6b 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -104,8 +104,7 @@ type_map = {'int8': 'int8_t',
   'uint32': 'uint32_t',
   'uint64': 'uint64_t',
   'float32': 'float',
-  'bool': 'bool',
-  'actuator_controls': 'actuator_controls'}
+  'bool': 'bool'}
 
 # Function to print a standard ros type
 def print_field_def(field):

From 9d5f06c9a779752cfb1662d5c794fa85fda3d494 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 8 Dec 2014 15:50:30 +0100
Subject: [PATCH 146/416] remove actuator armed uorb topic

---
 src/modules/uORB/topics/actuator_armed.h | 63 ------------------------
 1 file changed, 63 deletions(-)
 delete mode 100644 src/modules/uORB/topics/actuator_armed.h

diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
deleted file mode 100644
index 918a305fdd..0000000000
--- a/src/modules/uORB/topics/actuator_armed.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
- /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/actuator_armed.msg */
-
-
-#pragma once
-
-#include <stdint.h>
-#include "../uORB.h"
-
-
-
-/**
- * @addtogroup topics
- * @{
- */
-
-
-struct actuator_armed_s {
-	uint64_t timestamp;
-	bool armed;
-	bool ready_to_arm;
-	bool lockdown;
-	bool force_failsafe;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(actuator_armed);

From 76457e63c51695c814dda75c1cc351cb509c9a19 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 12:38:54 +0100
Subject: [PATCH 147/416] add nuttx platform to default makefile

---
 makefiles/config_px4fmu-v2_default.mk | 1 +
 1 file changed, 1 insertion(+)

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index df6b6d0c5b..38c5ebc7b3 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -113,6 +113,7 @@ MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
 
 #
 # OBC challenge

From 712e5797eba482592c372b06384e79d217f6bc05 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 12:39:33 +0100
Subject: [PATCH 148/416] Subscription: define more templates

---
 src/modules/uORB/Subscription.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 61609d0092..fa0594c2ef 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -95,5 +95,7 @@ template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
 template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
 template class __EXPORT Subscription<rc_channels_s>;
+template class __EXPORT Subscription<vehicle_control_mode_s>;
+template class __EXPORT Subscription<actuator_armed_s>;
 
 } // namespace uORB

From 9ed57211cc684d5135a254d9230c6d82bb092f9b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 12:40:36 +0100
Subject: [PATCH 149/416] hack to define isspace in px4_defines, add macro for
 subscription without callback

---
 src/platforms/px4_defines.h | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index ff302448a5..283dc5a53c 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -96,6 +96,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
  * Building for NuttX
  */
 #include <platforms/px4_includes.h>
+#ifdef __cplusplus
+#include <functional>
+#endif
 /* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
 
@@ -113,6 +116,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), nullptr, _interval)
 
 /* Parameter handle datatype */
 #include <systemlib/param/param.h>
@@ -126,10 +130,10 @@ typedef param_t px4_param_t;
 #endif
 
 /* Shortcut for subscribing to topics
- * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback
+ * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all
  */
 #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
-#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
+#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__)
 
 /* shortcut for advertising topics */
 #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
@@ -139,3 +143,7 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
+
+#define isspace(c) \
+  ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
+   (c) == '\r' || (c) == '\f' || c== '\v')

From 24fd5759b5443cf198f29eb6d5eae8c80cb04fe0 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 12:41:11 +0100
Subject: [PATCH 150/416] add missing __EXPORT

---
 src/platforms/px4_middleware.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index b0bc404173..16e73ec04a 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -59,7 +59,7 @@ extern bool task_should_exit;
 /**
  * Returns true if the app/task should continue to run
  */
-inline bool ok() { return !task_should_exit; }
+__EXPORT inline bool ok() { return !task_should_exit; }
 #endif
 
 class Rate

From e6224304606e20bbf79280978ba70326eabea575 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 12:41:54 +0100
Subject: [PATCH 151/416] px4 subscriber/nuttx: don't call null callback

---
 src/platforms/px4_subscriber.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index e995c6e49d..5d0120f90e 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -115,6 +115,10 @@ public:
 	 */
 	void update()
 	{
+		if (_callback == nullptr) {
+			return;
+		}
+
 		if (!uORB::Subscription<M>::updated()) {
 			/* Topic not updated, do not call callback */
 			return;

From 03ba38d0a4e39dc513cae6b53ff7cf845c13161c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 12:42:57 +0100
Subject: [PATCH 152/416] very much WIP, start to make mc att control p4 and
 ros compatible

---
 .../mc_att_control/mc_att_control_main.cpp    | 456 +++++++++---------
 1 file changed, 231 insertions(+), 225 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 631ca57e05..7a3b614825 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -71,17 +71,53 @@
 
 #include "mc_att_control_base.h"
 
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+
+using namespace px4;
+
 /**
  * Multicopter attitude control app start / stop handling function
  *
  * @ingroup apps
  */
-extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
+
+PX4_MAIN_FUNCTION(mc_att_control);
+
+int mc_attitude_thread_main(int argc, char *argv[]);
 
 #define YAW_DEADZONE	0.05f
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
+void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
+		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
+}
+
+
+namespace px4
+{
+bool task_should_exit = false;
+}
+
+// PX4_MAIN_FUNCTION(mc_att_control) {
+	// px4::init(argc, argv, "listener");
+
+	// px4::NodeHandle n;
+
+	// PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000);
+
+	/**
+	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
+	 * callbacks will be called from within this thread (the main one).  px4::spin()
+	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
+	 */
+	// n.spin();
+	// PX4_INFO("finished, returning");
+
+	// return 0;
+// }
+
 class MulticopterAttitudeControl :
 	public MulticopterAttitudeControlBase
 {
@@ -96,15 +132,10 @@ public:
 	 */
 	~MulticopterAttitudeControl();
 
-	/**
-	 * Start the sensors task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
-
 	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
 
+	void spin() { n.spin(); }
+
 private:
 	bool		_task_should_exit;			/**< if true, sensor task should exit */
 	int		_control_task;				/**< task handle for sensor task */
@@ -123,6 +154,8 @@ private:
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
+	px4::NodeHandle n;
+
 	struct {
 		param_t roll_p;
 		param_t roll_rate_p;
@@ -184,15 +217,6 @@ private:
 	 */
 	void		arming_status_poll();
 
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main attitude control task.
-	 */
-	void		task_main();
 };
 
 namespace mc_att_control
@@ -204,7 +228,6 @@ namespace mc_att_control
 #endif
 static const int ERROR = -1;
 
-MulticopterAttitudeControl	*g_control;
 }
 
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
@@ -224,6 +247,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
+	n(),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -252,6 +276,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	/* fetch initial parameter values */
 	parameters_update();
+
+	/*
+	 * do subscriptions
+	 */
+	// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0);
+	// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0);
+	// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	PX4_SUBSCRIBE(n, vehicle_control_mode, 0);
+	// _params_sub = orb_subscribe(ORB_ID(parameter_update));
+	PX4_SUBSCRIBE(n, parameter_update, 0);
+	// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	PX4_SUBSCRIBE(n, manual_control_setpoint, 0);
+	// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	PX4_SUBSCRIBE(n, actuator_armed, 0);
+
 }
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -275,7 +318,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 		} while (_control_task != -1);
 	}
 
-	mc_att_control::g_control = nullptr;
+	// mc_att_control::g_control = nullptr;
 }
 
 int
@@ -415,200 +458,140 @@ MulticopterAttitudeControl::arming_status_poll()
 	}
 }
 
-void
-MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
-{
-	mc_att_control::g_control->task_main();
-}
+// void
+// MulticopterAttitudeControl::task_main()
+// {
 
-void
-MulticopterAttitudeControl::task_main()
-{
-	px4::NodeHandle n;
 
-	/*
-	 * do subscriptions
-	 */
-	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	// PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, MulticopterAttitudeControl::handle_vehicle_attitude, this 0);
-	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	// [> wakeup source: vehicle attitude <]
+	// struct pollfd fds[1];
 
-	/* initialize parameters cache */
-	parameters_update();
+	// fds[0].fd = _v_att_sub;
+	// fds[0].events = POLLIN;
 
-	/* wakeup source: vehicle attitude */
-	struct pollfd fds[1];
+	// while (!_task_should_exit) {
 
-	fds[0].fd = _v_att_sub;
-	fds[0].events = POLLIN;
 
-	while (!_task_should_exit) {
+		// perf_end(_loop_perf);
+	// }
 
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+	// warnx("exit");
 
-		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0) {
-			continue;
-		}
-
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
-			warn("poll error %d, %d", pret, errno);
-			/* sleep a bit before next try */
-			usleep(100000);
-			continue;
-		}
-
-		perf_begin(_loop_perf);
-
-		/* run controller on attitude changes */
-		if (fds[0].revents & POLLIN) {
-			static uint64_t last_run = 0;
-			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
-
-			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
-			if (dt < 0.002f) {
-				dt = 0.002f;
-
-			} else if (dt > 0.02f) {
-				dt = 0.02f;
-			}
-
-			/* copy attitude topic */
-			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
-
-			/* check for updates in other topics */
-			parameter_update_poll();
-			vehicle_control_mode_poll();
-			arming_status_poll();
-			vehicle_manual_poll();
-
-			if (_v_control_mode.flag_control_attitude_enabled) {
-				control_attitude(dt);
-
-				/* publish the attitude setpoint if needed */
-				if (_publish_att_sp) {
-					_v_att_sp.timestamp = hrt_absolute_time();
-
-					if (_att_sp_pub > 0) {
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-							    &_v_att_sp);
-
-					} else {
-						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-									    &_v_att_sp);
-					}
-				}
-
-				/* publish attitude rates setpoint */
-				_v_rates_sp.roll = _rates_sp(0);
-				_v_rates_sp.pitch = _rates_sp(1);
-				_v_rates_sp.yaw = _rates_sp(2);
-				_v_rates_sp.thrust = _thrust_sp;
-				_v_rates_sp.timestamp = hrt_absolute_time();
-
-				if (_v_rates_sp_pub > 0) {
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-				} else {
-					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-				}
-
-			} else {
-				/* attitude controller disabled, poll rates setpoint topic */
-				if (_v_control_mode.flag_control_manual_enabled) {
-					/* manual rates control - ACRO mode */
-					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
-								    _manual_control_sp.r).emult(_params.acro_rate_max);
-					_thrust_sp = _manual_control_sp.z;
-
-					/* reset yaw setpoint after ACRO */
-					_reset_yaw_sp = true;
-
-					/* publish attitude rates setpoint */
-					_v_rates_sp.roll = _rates_sp(0);
-					_v_rates_sp.pitch = _rates_sp(1);
-					_v_rates_sp.yaw = _rates_sp(2);
-					_v_rates_sp.thrust = _thrust_sp;
-					_v_rates_sp.timestamp = hrt_absolute_time();
-
-					if (_v_rates_sp_pub > 0) {
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-					} else {
-						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-					}
-
-				} else {
-					/* attitude controller disabled, poll rates setpoint topic */
-					vehicle_rates_setpoint_poll();
-					_rates_sp(0) = _v_rates_sp.roll;
-					_rates_sp(1) = _v_rates_sp.pitch;
-					_rates_sp(2) = _v_rates_sp.yaw;
-					_thrust_sp = _v_rates_sp.thrust;
-				}
-			}
-
-			if (_v_control_mode.flag_control_rates_enabled) {
-				control_attitude_rates(dt);
-
-				/* publish actuator controls */
-				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
-				_actuators.timestamp = hrt_absolute_time();
-
-				if (!_actuators_0_circuit_breaker_enabled) {
-					if (_actuators_0_pub > 0) {
-						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
-
-					} else {
-						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
-					}
-				}
-			}
-		}
-
-		perf_end(_loop_perf);
-	}
-
-	warnx("exit");
-
-	_control_task = -1;
-	_exit(0);
-}
+	// _control_task = -1;
+	// _exit(0);
+// }
 
 void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
-		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
-}
 
-int
-MulticopterAttitudeControl::start()
-{
-	ASSERT(_control_task == -1);
+	perf_begin(_loop_perf);
 
-	/* start the task */
-	_control_task = task_spawn_cmd("mc_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
-				       nullptr);
+	/* run controller on attitude changes */
+	static uint64_t last_run = 0;
+	float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+	last_run = hrt_absolute_time();
 
-	if (_control_task < 0) {
-		warn("task start failed");
-		return -errno;
+	/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+	if (dt < 0.002f) {
+		dt = 0.002f;
+
+	} else if (dt > 0.02f) {
+		dt = 0.02f;
 	}
 
-	return OK;
+	/* copy attitude topic */
+	orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+	/* check for updates in other topics */
+	parameter_update_poll();
+	vehicle_control_mode_poll();
+	arming_status_poll();
+	vehicle_manual_poll();
+
+	if (_v_control_mode.flag_control_attitude_enabled) {
+		control_attitude(dt);
+
+		/* publish the attitude setpoint if needed */
+		if (_publish_att_sp) {
+			_v_att_sp.timestamp = hrt_absolute_time();
+
+			if (_att_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+					    &_v_att_sp);
+
+			} else {
+				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+							    &_v_att_sp);
+			}
+		}
+
+		/* publish attitude rates setpoint */
+		_v_rates_sp.roll = _rates_sp(0);
+		_v_rates_sp.pitch = _rates_sp(1);
+		_v_rates_sp.yaw = _rates_sp(2);
+		_v_rates_sp.thrust = _thrust_sp;
+		_v_rates_sp.timestamp = hrt_absolute_time();
+
+		if (_v_rates_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+		} else {
+			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+		}
+
+	} else {
+		/* attitude controller disabled, poll rates setpoint topic */
+		if (_v_control_mode.flag_control_manual_enabled) {
+			/* manual rates control - ACRO mode */
+			_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
+						    _manual_control_sp.r).emult(_params.acro_rate_max);
+			_thrust_sp = _manual_control_sp.z;
+
+			/* reset yaw setpoint after ACRO */
+			_reset_yaw_sp = true;
+
+			/* publish attitude rates setpoint */
+			_v_rates_sp.roll = _rates_sp(0);
+			_v_rates_sp.pitch = _rates_sp(1);
+			_v_rates_sp.yaw = _rates_sp(2);
+			_v_rates_sp.thrust = _thrust_sp;
+			_v_rates_sp.timestamp = hrt_absolute_time();
+
+			if (_v_rates_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+			} else {
+				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+			}
+
+		} else {
+			/* attitude controller disabled, poll rates setpoint topic */
+			vehicle_rates_setpoint_poll();
+			_rates_sp(0) = _v_rates_sp.roll;
+			_rates_sp(1) = _v_rates_sp.pitch;
+			_rates_sp(2) = _v_rates_sp.yaw;
+			_thrust_sp = _v_rates_sp.thrust;
+		}
+	}
+
+	if (_v_control_mode.flag_control_rates_enabled) {
+		control_attitude_rates(dt);
+
+		/* publish actuator controls */
+		_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.timestamp = hrt_absolute_time();
+
+		if (!_actuators_0_circuit_breaker_enabled) {
+			if (_actuators_0_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+
+			} else {
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			}
+		}
+	}
 }
 
 PX4_MAIN_FUNCTION(mc_att_control)
@@ -621,44 +604,67 @@ PX4_MAIN_FUNCTION(mc_att_control)
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (mc_att_control::g_control != nullptr) {
-			errx(1, "already running");
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
 		}
 
-		mc_att_control::g_control = new MulticopterAttitudeControl;
+		task_should_exit = false;
 
-		if (mc_att_control::g_control == nullptr) {
-			errx(1, "alloc failed");
-		}
-
-		if (OK != mc_att_control::g_control->start()) {
-			delete mc_att_control::g_control;
-			mc_att_control::g_control = nullptr;
-			err(1, "start failed");
-		}
+		daemon_task = task_spawn_cmd("mc_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       mc_attitude_thread_main,
+					(argv) ? (const char **)&argv[2] : (const char **)NULL);
 
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (mc_att_control::g_control == nullptr) {
-			errx(1, "not running");
-		}
+	// if (!strcmp(argv[1], "stop")) {
+		// if (mc_att_control::g_control == nullptr) {
+			// errx(1, "not running");
+		// }
 
-		delete mc_att_control::g_control;
-		mc_att_control::g_control = nullptr;
-		exit(0);
-	}
+		// delete mc_att_control::g_control;
+		// mc_att_control::g_control = nullptr;
+		// exit(0);
+	// }
 
-	if (!strcmp(argv[1], "status")) {
-		if (mc_att_control::g_control) {
-			errx(0, "running");
+	// if (!strcmp(argv[1], "status")) {
+		// if (mc_att_control::g_control) {
+			// errx(0, "running");
 
-		} else {
-			errx(1, "not running");
-		}
-	}
+		// } else {
+			// errx(1, "not running");
+		// }
+	// }
 
 	warnx("unrecognized command");
 	return 1;
 }
+
+int mc_attitude_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	MulticopterAttitudeControl attctl;
+
+	thread_running = true;
+
+	attctl.spin();
+
+	// while (!task_should_exit) {
+		// attctl.update();
+	// }
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}
+
+

From e43a05de3af19f8390aaf5432e7308cc5dabe5ff Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 15:06:17 +0100
Subject: [PATCH 153/416] add systemlib to px4 includes

---
 src/platforms/px4_includes.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index b8c8d034ed..525f94aae9 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -65,5 +65,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
 
 #endif

From cb77c16601338d2d0b22dc71ef057ec4d91652c4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 15:06:45 +0100
Subject: [PATCH 154/416] add sched.h to systemlib includes

SCHED_RR and SCHED_FIFO are defined in sched.h
---
 src/modules/systemlib/systemlib.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 3728f20672..6e22a557ea 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -41,6 +41,7 @@
 #define SYSTEMLIB_H_
 #include <float.h>
 #include <stdint.h>
+#include <sched.h>
 
 __BEGIN_DECLS
 

From 8ae1c9763d649470ae7c55e35507509dbb2612ff Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 16:22:19 +0100
Subject: [PATCH 155/416] write publisher example as class

---
 src/examples/publisher/module.mk              |   3 +-
 src/examples/publisher/publisher.cpp          | 100 ---------------
 src/examples/publisher/publisher_example.cpp  |  67 ++++++++++
 src/examples/publisher/publisher_example.h    |  51 ++++++++
 src/examples/publisher/publisher_main.cpp     | 112 ++++++++++++++++
 src/examples/subscriber/module.mk             |   3 +-
 src/examples/subscriber/subscriber.cpp        | 120 ------------------
 .../subscriber/subscriber_example.cpp         |  78 ++++++++++++
 src/examples/subscriber/subscriber_example.h  |  58 +++++++++
 src/examples/subscriber/subscriber_main.cpp   | 112 ++++++++++++++++
 .../mc_att_control/mc_att_control_main.cpp    |   3 +-
 11 files changed, 483 insertions(+), 224 deletions(-)
 delete mode 100644 src/examples/publisher/publisher.cpp
 create mode 100644 src/examples/publisher/publisher_example.cpp
 create mode 100644 src/examples/publisher/publisher_example.h
 create mode 100644 src/examples/publisher/publisher_main.cpp
 delete mode 100644 src/examples/subscriber/subscriber.cpp
 create mode 100644 src/examples/subscriber/subscriber_example.cpp
 create mode 100644 src/examples/subscriber/subscriber_example.h
 create mode 100644 src/examples/subscriber/subscriber_main.cpp

diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk
index 0fc4316ecb..4f941cd439 100644
--- a/src/examples/publisher/module.mk
+++ b/src/examples/publisher/module.mk
@@ -37,6 +37,7 @@
 
 MODULE_COMMAND	= publisher
 
-SRCS		= publisher.cpp
+SRCS		= publisher_main.cpp \
+		  publisher_example.cpp
 
 MODULE_STACKSIZE = 1200
diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
deleted file mode 100644
index 8ef147493f..0000000000
--- a/src/examples/publisher/publisher.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *   * Redistributions of source code must retain the above copyright notice,
- *     this list of conditions and the following disclaimer.
- *   * 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.
- *   * Neither the names of Stanford University or Willow Garage, Inc. 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.
- */
-
-#include <px4.h>
-
-using namespace px4;
-
-/**
- * This tutorial demonstrates simple sending of messages over the PX4 middleware system.
- */
-
-namespace px4
-{
-bool task_should_exit = false;
-}
-
-PX4_MAIN_FUNCTION(publisher)
-{
-
-	px4::init(argc, argv, "px4_publisher");
-
-	px4::NodeHandle n;
-
-	/**
-	 * The advertise() function is how you tell ROS that you want to
-	 * publish on a given topic name. This invokes a call to the ROS
-	 * master node, which keeps a registry of who is publishing and who
-	 * is subscribing. After this advertise() call is made, the master
-	 * node will notify anyone who is trying to subscribe to this topic name,
-	 * and they will in turn negotiate a peer-to-peer connection with this
-	 * node.  advertise() returns a Publisher object which allows you to
-	 * publish messages on that topic through a call to publish().  Once
-	 * all copies of the returned Publisher object are destroyed, the topic
-	 * will be automatically unadvertised.
-	 *
-	 * The second parameter to advertise() is the size of the message queue
-	 * used for publishing messages.  If messages are published more quickly
-	 * than we can send them, the number here specifies how many messages to
-	 * buffer up before throwing some away.
-	 */
-	px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels);
-
-
-	px4::Rate loop_rate(10);
-
-	/**
-	 * A count of how many messages we have sent. This is used to create
-	 * a unique string for each message.
-	 */
-	int count = 0;
-
-	while (px4::ok()) {
-		/**
-		 * This is a message object. You stuff it with data, and then publish it.
-		 */
-		PX4_TOPIC_T(rc_channels) msg;
-
-		msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.timestamp_last_valid);
-
-		/**
-		 * The publish() function is how you send messages. The parameter
-		 * is the message object. The type of this object must agree with the type
-		 * given as a template parameter to the advertise<>() call, as was done
-		 * in the constructor above.
-		 */
-		rc_channels_pub->publish(msg);
-
-		n.spinOnce();
-
-		loop_rate.sleep();
-		++count;
-	}
-
-	return 0;
-}
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
new file mode 100644
index 0000000000..3c716291b7
--- /dev/null
+++ b/src/examples/publisher/publisher_example.cpp
@@ -0,0 +1,67 @@
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "publisher_example.h"
+
+PublisherExample::PublisherExample() :
+	_n(),
+	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
+{
+
+}
+
+int PublisherExample::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (px4::ok()) {
+		PX4_TOPIC_T(rc_channels) msg;
+		msg.timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("%llu", msg.timestamp_last_valid);
+
+		_rc_channels_pub->publish(msg);
+
+		_n.spinOnce();
+		loop_rate.sleep();
+	}
+
+	return 0;
+}
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
new file mode 100644
index 0000000000..78c1ffc89f
--- /dev/null
+++ b/src/examples/publisher/publisher_example.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher.h
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <px4.h>
+class PublisherExample {
+public:
+	PublisherExample();
+
+	~PublisherExample() {};
+
+	int main();
+protected:
+	px4::NodeHandle _n;
+	px4::Publisher * _rc_channels_pub;
+};
diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
new file mode 100644
index 0000000000..e1034fec53
--- /dev/null
+++ b/src/examples/publisher/publisher_main.cpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_main.cpp
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include "publisher_example.h"
+
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+int publisher_task_main(int argc, char *argv[]);
+
+PX4_MAIN_FUNCTION(publisher)
+{
+	px4::init(argc, argv, "publisher");
+
+	if (argc < 1) {
+		errx(1, "usage: publisher {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("publisher",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       publisher_task_main,
+					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
+
+int publisher_task_main(int argc, char *argv[])
+{
+	warnx("starting");
+	PublisherExample p;
+	thread_running = true;
+	p.main();
+
+	warnx("exiting.");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
index 90b4d8ffc5..3156ebb30a 100644
--- a/src/examples/subscriber/module.mk
+++ b/src/examples/subscriber/module.mk
@@ -37,7 +37,8 @@
 
 MODULE_COMMAND	= subscriber
 
-SRCS		= subscriber.cpp \
+SRCS		= subscriber_main.cpp \
+		  subscriber_example.cpp \
 		  subscriber_params.c
 
 MODULE_STACKSIZE = 2400
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
deleted file mode 100644
index 0d9ca1fa76..0000000000
--- a/src/examples/subscriber/subscriber.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *   * Redistributions of source code must retain the above copyright notice,
- *     this list of conditions and the following disclaimer.
- *   * 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.
- *   * Neither the names of Stanford University or Willow Garage, Inc. 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.
- */
-
-#include <px4.h>
-#include "subscriber_params.h"
-#include <cstdio>
-
-using namespace px4;
-
-/**
- * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
- */
-void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
-}
-void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid);
-}
-
-class RCHandler {
-public:
-	void  callback(const PX4_TOPIC_T(rc_channels) &msg) {
-		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid);
-	}
-};
-
-RCHandler rchandler;
-
-
-namespace px4
-{
-bool task_should_exit = false;
-}
-
-PX4_MAIN_FUNCTION(subscriber) {
-	/**
-	 * The ros::init() function needs to see argc and argv so that it can perform
-	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
-	 * remappings you can use a different version of init() which takes remappings
-	 * directly, but for most command-line programs, passing argc and argv is the easiest
-	 * way to do it.  The third argument to init() is the name of the node.
-	 *
-	 * You must call one of the versions of px4::init() before using any other
-	 * part of the PX4/ ROS system.
-	 */
-	px4::init(argc, argv, "listener");
-
-	/**
-	 * NodeHandle is the main access point to communications with the ROS system.
-	 * The first NodeHandle constructed will fully initialize this node, and the last
-	 * NodeHandle destructed will close down the node.
-	 */
-	px4::NodeHandle n;
-
-	/* Define parameters */
-	px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV);
-	px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF);
-
-	/* Read the parameter back for testing */
-	int32_t sub_interval;
-	float test_float;
-	PX4_PARAM_GET(p_sub_interv, &sub_interval);
-	PX4_INFO("Param SUB_INTERV = %d", sub_interval);
-	PX4_PARAM_GET(p_test_float, &test_float);
-	PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float);
-
-	/**
-	 * The subscribe() call is how you tell ROS that you want to receive messages
-	 * on a given topic.  This invokes a call to the ROS
-	 * master node, which keeps a registry of who is publishing and who
-	 * is subscribing.  Messages are passed to a callback function, here
-	 * called chatterCallback.  subscribe() returns a Subscriber object that you
-	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
-	 * object go out of scope, this callback will automatically be unsubscribed from
-	 * this topic.
-	 *
-	 * The second parameter to the subscribe() function is the size of the message
-	 * queue.  If messages are arriving faster than they are being processed, this
-	 * is the number of messages that will be buffered up before beginning to throw
-	 * away the oldest ones.
-	 */
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval);
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
-	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
-	PX4_INFO("subscribed");
-
-	/**
-	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
-	 * callbacks will be called from within this thread (the main one).  px4::spin()
-	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
-	 */
-	n.spin();
-	PX4_INFO("finished, returning");
-
-	return 0;
-}
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
new file mode 100644
index 0000000000..cddab10355
--- /dev/null
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "subscriber_params.h"
+#include "subscriber_example.h"
+
+using namespace px4;
+
+void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
+	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
+}
+
+SubscriberExample::SubscriberExample() :
+	_n(),
+	_sub_interval(0),
+	_test_float(0.0f)
+{
+	/* Define parameters */
+	_p_sub_interv = PX4_PARAM_INIT(SUB_INTERV);
+	_p_test_float = PX4_PARAM_INIT(SUB_TESTF);
+
+	/* Read the parameter back as example */
+	PX4_PARAM_GET(_p_sub_interv, &_sub_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _sub_interval);
+	PX4_PARAM_GET(_p_test_float, &_test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
+
+	/* Do some subscriptions */
+	/* Function */
+	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _sub_interval);
+	/* Class Method */
+	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+	PX4_INFO("subscribed");
+}
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
+ */
+void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
+	PX4_INFO("Subscriber callback: [%llu]", msg.timestamp_last_valid);
+}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
new file mode 100644
index 0000000000..4bdc15c2d7
--- /dev/null
+++ b/src/examples/subscriber/subscriber_example.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber.h
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <px4.h>
+class SubscriberExample {
+public:
+	SubscriberExample();
+
+	~SubscriberExample() {};
+
+	void spin() {_n.spin();}
+protected:
+	px4::NodeHandle _n;
+	px4_param_t _p_sub_interv;
+	int32_t _sub_interval;
+	px4_param_t _p_test_float;
+	float _test_float;
+
+	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
+
+
+};
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
new file mode 100644
index 0000000000..32de4fd6eb
--- /dev/null
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_main.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include "subscriber_example.h"
+
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+int subscriber_task_main(int argc, char *argv[]);
+
+PX4_MAIN_FUNCTION(subscriber)
+{
+	px4::init(argc, argv, "subscriber");
+
+	if (argc < 1) {
+		errx(1, "usage: subscriber {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("subscriber",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       subscriber_task_main,
+					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
+
+int subscriber_task_main(int argc, char *argv[])
+{
+	warnx("starting");
+	SubscriberExample s;
+	thread_running = true;
+	s.spin();
+
+	warnx("exiting.");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 7a3b614825..1103191e7f 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -100,8 +100,7 @@ namespace px4
 bool task_should_exit = false;
 }
 
-// PX4_MAIN_FUNCTION(mc_att_control) {
-	// px4::init(argc, argv, "listener");
+// PX4_MAIN_FUNCTION(mc_att_control) { px4::init(argc, argv, "listener");
 
 	// px4::NodeHandle n;
 

From da4cfad3c2dc14f17ccf0e9a8ce41309b76964e4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 16:25:10 +0100
Subject: [PATCH 156/416] subscriber example: improve init

---
 src/examples/subscriber/subscriber_example.cpp | 8 +++-----
 src/examples/subscriber/subscriber_example.h   | 2 +-
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index cddab10355..e6d63cab33 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file subscriber.cpp
+ * @file subscriber_example.cpp
  * Example subscriber for ros and px4
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
@@ -49,13 +49,11 @@ void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
 
 SubscriberExample::SubscriberExample() :
 	_n(),
+	_p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)),
 	_sub_interval(0),
+	_p_test_float(PX4_PARAM_INIT(SUB_TESTF)),
 	_test_float(0.0f)
 {
-	/* Define parameters */
-	_p_sub_interv = PX4_PARAM_INIT(SUB_INTERV);
-	_p_test_float = PX4_PARAM_INIT(SUB_TESTF);
-
 	/* Read the parameter back as example */
 	PX4_PARAM_GET(_p_sub_interv, &_sub_interval);
 	PX4_INFO("Param SUB_INTERV = %d", _sub_interval);
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 4bdc15c2d7..b52f89ba87 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file subscriber.h
+ * @file subscriber_example.h
  * Example subscriber for ros and px4
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>

From f0ad2c9ef55dc2508e777384f8d0496042ade9e8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 17:02:49 +0100
Subject: [PATCH 157/416] px4 subscriber: uorb: check if callback null at
 correct location

---
 src/platforms/px4_subscriber.h | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 5d0120f90e..45cb650b24 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -115,10 +115,6 @@ public:
 	 */
 	void update()
 	{
-		if (_callback == nullptr) {
-			return;
-		}
-
 		if (!uORB::Subscription<M>::updated()) {
 			/* Topic not updated, do not call callback */
 			return;
@@ -127,6 +123,12 @@ public:
 		/* get latest data */
 		uORB::Subscription<M>::update();
 
+
+		/* Check if there is a callback */
+		if (_callback == nullptr) {
+			return;
+		}
+
 		/* Call callback which performs actions based on this data */
 		_callback(uORB::Subscription<M>::getData());
 

From 1b6b6cd35c1f14766d49144bbc18d19d02656362 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 17:03:24 +0100
Subject: [PATCH 158/416] add no callback example to subscriber example

---
 src/examples/subscriber/subscriber_example.cpp | 15 ++++++++++-----
 src/examples/subscriber/subscriber_example.h   |  3 ++-
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index e6d63cab33..460a3267a1 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -50,21 +50,24 @@ void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
 SubscriberExample::SubscriberExample() :
 	_n(),
 	_p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)),
-	_sub_interval(0),
+	_interval(0),
 	_p_test_float(PX4_PARAM_INIT(SUB_TESTF)),
 	_test_float(0.0f)
 {
 	/* Read the parameter back as example */
-	PX4_PARAM_GET(_p_sub_interv, &_sub_interval);
-	PX4_INFO("Param SUB_INTERV = %d", _sub_interval);
+	PX4_PARAM_GET(_p_sub_interv, &_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _interval);
 	PX4_PARAM_GET(_p_test_float, &_test_float);
 	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
 
 	/* Do some subscriptions */
 	/* Function */
-	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _sub_interval);
+	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
 	/* Class Method */
 	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+	/* No callback */
+	_sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+
 	PX4_INFO("subscribed");
 }
 
@@ -72,5 +75,7 @@ SubscriberExample::SubscriberExample() :
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  */
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("Subscriber callback: [%llu]", msg.timestamp_last_valid);
+	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			msg.timestamp_last_valid,
+			((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
 }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index b52f89ba87..848d43f76a 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -48,9 +48,10 @@ public:
 protected:
 	px4::NodeHandle _n;
 	px4_param_t _p_sub_interv;
-	int32_t _sub_interval;
+	int32_t _interval;
 	px4_param_t _p_test_float;
 	float _test_float;
+	px4::Subscriber * _sub_rc_chan;
 
 	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 

From d04bbf11ec5d273902d3920981ccac49489d7098 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 10 Dec 2014 18:34:41 +0100
Subject: [PATCH 159/416] subscriber example: add comment

---
 src/examples/subscriber/subscriber_example.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 460a3267a1..4e0350ff0f 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -73,6 +73,7 @@ SubscriberExample::SubscriberExample() :
 
 /**
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
+ * Also the current value of the _sub_rc_chan subscription is printed
  */
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",

From a1685ed6d093ec01214e6839ea66f9dd565e55ce Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 09:27:11 +0100
Subject: [PATCH 160/416] change definition of px4 main function

---
 src/examples/publisher/publisher_main.cpp   |  6 +++---
 src/examples/subscriber/subscriber_main.cpp | 10 +++++-----
 src/platforms/px4_defines.h                 |  2 +-
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index e1034fec53..c4e79aaa6b 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -49,9 +49,9 @@ bool task_should_exit = false;
 }
 using namespace px4;
 
-int publisher_task_main(int argc, char *argv[]);
+PX4_MAIN_FUNCTION(publisher);
 
-PX4_MAIN_FUNCTION(publisher)
+extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 {
 	px4::init(argc, argv, "publisher");
 
@@ -99,7 +99,7 @@ PX4_MAIN_FUNCTION(publisher)
 	return 1;
 }
 
-int publisher_task_main(int argc, char *argv[])
+PX4_MAIN_FUNCTION(publisher)
 {
 	warnx("starting");
 	PublisherExample p;
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
index 32de4fd6eb..0436dc9f2b 100644
--- a/src/examples/subscriber/subscriber_main.cpp
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -49,12 +49,10 @@ bool task_should_exit = false;
 }
 using namespace px4;
 
-int subscriber_task_main(int argc, char *argv[]);
+PX4_MAIN_FUNCTION(subscriber);
 
-PX4_MAIN_FUNCTION(subscriber)
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
 {
-	px4::init(argc, argv, "subscriber");
-
 	if (argc < 1) {
 		errx(1, "usage: subscriber {start|stop|status}");
 	}
@@ -99,8 +97,10 @@ PX4_MAIN_FUNCTION(subscriber)
 	return 1;
 }
 
-int subscriber_task_main(int argc, char *argv[])
+PX4_MAIN_FUNCTION(subscriber)
 {
+	px4::init(argc, argv, "subscriber");
+
 	warnx("starting");
 	SubscriberExample s;
 	thread_running = true;
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 283dc5a53c..440f1b6fcd 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -100,7 +100,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #include <functional>
 #endif
 /* Main entry point */
-#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
+#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[])
 
 /* Print/output wrappers */
 #define PX4_WARN warnx

From 173b1b2a8bcf2344be92b1f6e5acbc089a43fcab Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 10:51:19 +0100
Subject: [PATCH 161/416] WIP, make class based and extended
 subscriber/publisher example compile for ros

---
 CMakeLists.txt                                | 24 +++++++++++--------
 src/examples/publisher/publisher_example.cpp  |  2 ++
 src/examples/publisher/publisher_example.h    |  1 +
 src/examples/publisher/publisher_main.cpp     |  6 +++--
 .../subscriber/subscriber_example.cpp         |  7 +++---
 src/examples/subscriber/subscriber_example.h  |  3 +++
 src/examples/subscriber/subscriber_main.cpp   |  6 +++--
 src/platforms/px4_defines.h                   |  7 ++++--
 src/platforms/px4_nodehandle.h                | 13 ++++++++++
 9 files changed, 50 insertions(+), 19 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e91f2cc371..f206a5aa2e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -111,7 +111,9 @@ target_link_libraries(px4
 )
 
 ## Declare a test publisher
-add_executable(publisher src/examples/publisher/publisher.cpp)
+add_executable(publisher
+	src/examples/publisher/publisher_main.cpp
+	src/examples/publisher/publisher_example.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -124,7 +126,9 @@ target_link_libraries(publisher
 )
 
 ## Declare a test subscriber
-add_executable(subscriber src/examples/subscriber/subscriber.cpp)
+add_executable(subscriber
+	src/examples/subscriber/subscriber_main.cpp
+	src/examples/subscriber/subscriber_example.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -136,14 +140,14 @@ target_link_libraries(subscriber
   px4
 )
 
-add_executable(mc_att_control
-	src/modules/mc_att_control/mc_att_control_main.cpp
-	src/moudles/mc_att_control/mc_att_control_base.cpp)
-add_dependencies(mc_att_control px4_generate_messages_cpp)
-target_link_libraries(mc_att_control
-  ${catkin_LIBRARIES}
-  px4
-)
+# add_executable(mc_att_control
+	# src/modules/mc_att_control/mc_att_control_main.cpp
+	# src/moudles/mc_att_control/mc_att_control_base.cpp)
+# add_dependencies(mc_att_control px4_generate_messages_cpp)
+# target_link_libraries(mc_att_control
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
 
 #############
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index 3c716291b7..2e5779ebec 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -41,6 +41,8 @@
 
 #include "publisher_example.h"
 
+using namespace px4;
+
 PublisherExample::PublisherExample() :
 	_n(),
 	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 78c1ffc89f..304ecef47a 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -38,6 +38,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 #include <px4.h>
+
 class PublisherExample {
 public:
 	PublisherExample();
diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index c4e79aaa6b..8b692d9633 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -51,6 +51,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(publisher);
 
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 {
 	px4::init(argc, argv, "publisher");
@@ -98,15 +99,16 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 	warnx("unrecognized command");
 	return 1;
 }
+#endif
 
 PX4_MAIN_FUNCTION(publisher)
 {
-	warnx("starting");
+	PX4_INFO("starting");
 	PublisherExample p;
 	thread_running = true;
 	p.main();
 
-	warnx("exiting.");
+	PX4_INFO("exiting.");
 	thread_running = false;
 	return 0;
 }
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 4e0350ff0f..39d0907520 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -76,7 +76,8 @@ SubscriberExample::SubscriberExample() :
  * Also the current value of the _sub_rc_chan subscription is printed
  */
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			msg.timestamp_last_valid,
-			((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
+	//XXX
+	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			// msg.timestamp_last_valid,
+			// ((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
 }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 848d43f76a..73c7390352 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -38,6 +38,9 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 #include <px4.h>
+
+using namespace px4;
+
 class SubscriberExample {
 public:
 	SubscriberExample();
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
index 0436dc9f2b..716233739b 100644
--- a/src/examples/subscriber/subscriber_main.cpp
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -51,6 +51,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(subscriber);
 
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
 {
 	if (argc < 1) {
@@ -96,17 +97,18 @@ extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
 	warnx("unrecognized command");
 	return 1;
 }
+#endif
 
 PX4_MAIN_FUNCTION(subscriber)
 {
 	px4::init(argc, argv, "subscriber");
 
-	warnx("starting");
+	PX4_INFO("starting");
 	SubscriberExample s;
 	thread_running = true;
 	s.spin();
 
-	warnx("exiting.");
+	PX4_INFO("exiting.");
 	thread_running = false;
 	return 0;
 }
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 440f1b6fcd..819954d0ef 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -66,9 +66,11 @@
 #define PX4_TOPIC_T(_name) _name
 
 /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr);
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<const PX4_TOPIC_T(_name)&>(PX4_TOPIC(_name));
 
 /* Parameter handle datatype */
 typedef const char *px4_param_t;
@@ -113,9 +115,10 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #define PX4_TOPIC_T(_name) _name##_s
 
 /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval)
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), nullptr, _interval)
 
 /* Parameter handle datatype */
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 5b7247b203..25b8e037d6 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -99,6 +99,19 @@ public:
 		return sub;
 	}
 
+	/**
+	 * Subscribe with no callback, just the latest value is stored on updates
+	 * @param topic		Name of the topic
+	 */
+	template<typename M>
+	Subscriber *subscribe(const char *topic)
+	{
+		//XXX missing implementation
+		// Subscriber *sub = new Subscriber(ros_sub);
+		// _subs.push_back(sub);
+		return (Subscriber *)NULL;
+	}
+
 	/**
 	 * Advertise topic
 	 * @param topic		Name of the topic

From fd6a5fd38b194300788c597fd1636b2a97e24642 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 10:58:47 +0100
Subject: [PATCH 162/416] move px4::init call

---
 src/examples/publisher/publisher_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index 8b692d9633..5b50ecc6ca 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -54,8 +54,6 @@ PX4_MAIN_FUNCTION(publisher);
 #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 {
-	px4::init(argc, argv, "publisher");
-
 	if (argc < 1) {
 		errx(1, "usage: publisher {start|stop|status}");
 	}
@@ -103,6 +101,8 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 
 PX4_MAIN_FUNCTION(publisher)
 {
+	px4::init(argc, argv, "publisher");
+
 	PX4_INFO("starting");
 	PublisherExample p;
 	thread_running = true;

From c68c277c94cacd2a64b634dd9c45ace2a04c2911 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 15:04:37 +0100
Subject: [PATCH 163/416] subscription class for ros now stores last message to
 avoid manual copy and support subscription with no callbacks

---
 CMakeLists.txt                                |   1 +
 .../subscriber/subscriber_example.cpp         |   7 +-
 src/platforms/px4_defines.h                   |  18 ++-
 src/platforms/px4_nodehandle.h                |  25 ++--
 src/platforms/px4_subscriber.h                | 113 +++++++++++++-----
 5 files changed, 116 insertions(+), 48 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f206a5aa2e..489467db7d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(px4)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 39d0907520..95e208ca6d 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -76,8 +76,7 @@ SubscriberExample::SubscriberExample() :
  * Also the current value of the _sub_rc_chan subscription is printed
  */
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	//XXX
-	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			// msg.timestamp_last_valid,
-			// ((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
+	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			msg.timestamp_last_valid,
+			((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid);
 }
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 819954d0ef..fa3ef216a5 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -70,7 +70,7 @@
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<const PX4_TOPIC_T(_name)&>(PX4_TOPIC(_name));
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name));
 
 /* Parameter handle datatype */
 typedef const char *px4_param_t;
@@ -93,6 +93,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER_T(_name) SubscriberROS<PX4_TOPIC_T(_name)>
+
 #else
 /*
  * Building for NuttX
@@ -130,6 +133,15 @@ typedef param_t px4_param_t;
 
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
+
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER_T(_name) SubscriberUORB<PX4_TOPIC_T(_name)>
+
+/* XXX this is a hack to resolve conflicts with NuttX headers */
+#define isspace(c) \
+  ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
+   (c) == '\r' || (c) == '\f' || c== '\v')
+
 #endif
 
 /* Shortcut for subscribing to topics
@@ -146,7 +158,3 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
-
-#define isspace(c) \
-  ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
-   (c) == '\r' || (c) == '\f' || c== '\v')
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 25b8e037d6..c24a183030 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -77,10 +77,11 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M>
-	Subscriber *subscribe(const char *topic, void(*fp)(M))
+	Subscriber *subscribe(const char *topic, void(*fp)(const M&))
 	{
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
-		Subscriber *sub = new Subscriber(ros_sub);
+		Subscriber *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
+		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
@@ -91,10 +92,11 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M, typename T>
-	Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj)
+	Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj)
 	{
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
-		Subscriber *sub = new Subscriber(ros_sub);
+		Subscriber *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
+		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
@@ -106,10 +108,11 @@ public:
 	template<typename M>
 	Subscriber *subscribe(const char *topic)
 	{
-		//XXX missing implementation
-		// Subscriber *sub = new Subscriber(ros_sub);
-		// _subs.push_back(sub);
-		return (Subscriber *)NULL;
+		Subscriber *sub = new SubscriberROS<M>();
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
+		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
+		_subs.push_back(sub);
+		return sub;
 	}
 
 	/**
@@ -165,7 +168,7 @@ public:
 			      std::function<void(const M &)> callback,
 			      unsigned interval)
 	{
-		SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
+		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, callback, &_subs);
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
 		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 45cb650b24..234b72f5b9 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -36,6 +36,7 @@
  *
  * PX4 Middleware Wrapper Subscriber
  */
+#include <functional>
 #pragma once
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /* includes when building for ros */
@@ -44,59 +45,108 @@
 /* includes when building for NuttX */
 #include <uORB/Subscription.hpp>
 #include <containers/List.hpp>
-#include <functional>
+#include "px4_nodehandle.h"
 #endif
 
 namespace px4
 {
-
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
-class Subscriber
-{
-public:
-	/**
-	 * Construct Subscriber by providing a ros::Subscriber
-	 * @param ros_sub the ros subscriber which will be used to perform the publications
-	 */
-	Subscriber(ros::Subscriber ros_sub) :
-		_ros_sub(ros_sub)
-	{}
-
-	~Subscriber() {};
-private:
-	ros::Subscriber _ros_sub;	/**< Handle to ros subscriber */
-};
-
-#else
-
 /**
- * Subscriber class which is used by nodehandle when building for NuttX
+ * Subscriber class which is used by nodehandle
  */
 class Subscriber
 {
 public:
 	Subscriber() {};
 	~Subscriber() {};
-private:
 };
 
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/**
+ * Subscriber class that is templated with the ros n message type
+ */
+template<typename M>
+class SubscriberROS :
+	public Subscriber
+{
+friend class NodeHandle;
+
+public:
+	/**
+	 * Construct Subscriber by providing a callback function
+	 */
+	SubscriberROS(std::function<void(const M &)> cbf) :
+		Subscriber(),
+		_ros_sub(),
+		_cbf(cbf),
+		_msg_current()
+	{}
+
+	/**
+	 * Construct Subscriber without a callback function
+	 */
+	SubscriberROS() :
+		Subscriber(),
+		_ros_sub(),
+		_cbf(NULL),
+		_msg_current()
+	{}
+
+
+	~SubscriberROS() {};
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	const M& get_msg() { return _msg_current; }
+
+protected:
+	/**
+	 * Called on topic update, saves the current message and then calls the provided callback function
+	 */
+	void callback(const M &msg) {
+		/* Store data */
+		_msg_current = msg;
+
+		/* Call callback */
+		if (_cbf != NULL) {
+			_cbf(msg);
+		}
+
+	}
+
+	/**
+	 * Saves the ros subscriber to keep ros subscription alive
+	 */
+	void set_ros_sub(ros::Subscriber ros_sub) {
+		_ros_sub = ros_sub;
+	}
+
+	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
+	std::function<void(const M &)> _cbf;	/**< Callback that the user provided on the subscription */
+	M _msg_current;				/**< Current Message value */
+
+};
+
+#else
+
 /**
  * Subscriber class that is templated with the uorb subscription message type
  */
 template<typename M>
-class SubscriberPX4 :
+class SubscriberUORB :
 	public Subscriber,
 	public uORB::Subscription<M>
 {
 public:
 	/**
-	 * Construct SubscriberPX4 by providing orb meta data
+	 * Construct SubscriberUORB by providing orb meta data
 	 * @param meta	    orb metadata for the topic which is used
 	 * @param callback	Callback, executed on receiving a new message
 	 * @param interval	Minimal interval between calls to callback
 	 * @param list	    subscriber is added to this list
 	 */
-	SubscriberPX4(const struct orb_metadata *meta,
+	SubscriberUORB(const struct orb_metadata *meta,
 		      unsigned interval,
 		      std::function<void(const M &)> callback,
 		      List<uORB::SubscriptionNode *> *list) :
@@ -106,7 +156,7 @@ public:
 		//XXX store callback
 	{}
 
-	~SubscriberPX4() {};
+	~SubscriberUORB() {};
 
 	/**
 	 * Update Subscription
@@ -133,7 +183,14 @@ public:
 		_callback(uORB::Subscription<M>::getData());
 
 	};
-private:
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	const M& get_msg() { return uORB::Subscription<M>::getData(); }
+
+protected:
 	std::function<void(const M &)> _callback;	/**< Callback handle,
 							  called when new data is available */
 

From 998646f03b229ae86bd6e57ff0bd15dd1763c342 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 15:33:32 +0100
Subject: [PATCH 164/416] add base class and template subscriber class as well
 to improve interface to get last msg value

---
 .../subscriber/subscriber_example.cpp         |  2 +-
 src/examples/subscriber/subscriber_example.h  |  2 +-
 src/platforms/px4_defines.h                   |  9 ++---
 src/platforms/px4_nodehandle.h                | 28 +++++++-------
 src/platforms/px4_publisher.h                 | 25 ++++++++++--
 src/platforms/px4_subscriber.h                | 38 +++++++++++++++----
 6 files changed, 70 insertions(+), 34 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 95e208ca6d..83ead4ba5a 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -78,5 +78,5 @@ SubscriberExample::SubscriberExample() :
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
 			msg.timestamp_last_valid,
-			((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid);
+			_sub_rc_chan->get_msg().timestamp_last_valid);
 }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 73c7390352..c4b853d4de 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -54,7 +54,7 @@ protected:
 	int32_t _interval;
 	px4_param_t _p_test_float;
 	float _test_float;
-	px4::Subscriber * _sub_rc_chan;
+	px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
 
 	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index fa3ef216a5..9bb190380e 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -93,9 +93,6 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 
-/* Get a subscriber class type based on the topic name */
-#define PX4_SUBSCRIBER_T(_name) SubscriberROS<PX4_TOPIC_T(_name)>
-
 #else
 /*
  * Building for NuttX
@@ -134,9 +131,6 @@ typedef param_t px4_param_t;
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 
-/* Get a subscriber class type based on the topic name */
-#define PX4_SUBSCRIBER_T(_name) SubscriberUORB<PX4_TOPIC_T(_name)>
-
 /* XXX this is a hack to resolve conflicts with NuttX headers */
 #define isspace(c) \
   ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
@@ -150,6 +144,9 @@ typedef param_t px4_param_t;
 #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
 #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__)
 
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER(_name) Subscriber<PX4_TOPIC_T(_name)>
+
 /* shortcut for advertising topics */
 #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
 
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index c24a183030..9bd792c697 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -77,13 +77,13 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M>
-	Subscriber *subscribe(const char *topic, void(*fp)(const M&))
+	Subscriber<M> *subscribe(const char *topic, void(*fp)(const M&))
 	{
-		Subscriber *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
+		SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
 		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
-		return sub;
+		return (Subscriber<M> *)sub;
 	}
 
 	/**
@@ -92,13 +92,13 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M, typename T>
-	Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj)
+	Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M&), T *obj)
 	{
-		Subscriber *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
+		SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
 		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
-		return sub;
+		return (Subscriber<M> *)sub;
 	}
 
 	/**
@@ -106,13 +106,13 @@ public:
 	 * @param topic		Name of the topic
 	 */
 	template<typename M>
-	Subscriber *subscribe(const char *topic)
+	Subscriber<M> *subscribe(const char *topic)
 	{
-		Subscriber *sub = new SubscriberROS<M>();
+		SubscriberBase *sub = new SubscriberROS<M>();
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
 		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
-		return sub;
+		return (Subscriber<M> *)sub;
 	}
 
 	/**
@@ -141,10 +141,10 @@ public:
 
 private:
 	static const uint32_t kQueueSizeDefault = 1000;		/**< Size of queue for ROS */
-	std::list<Subscriber *> _subs;				/**< Subcriptions of node */
-	std::list<Publisher *> _pubs;				/**< Publications of node */
+	std::list<SubscriberBase *> _subs;				/**< Subcriptions of node */
+	std::list<PublisherBase *> _pubs;				/**< Publications of node */
 };
-#else
+#else //Building for NuttX
 class __EXPORT NodeHandle
 {
 public:
@@ -164,7 +164,7 @@ public:
 	 */
 
 	template<typename M>
-	Subscriber *subscribe(const struct orb_metadata *meta,
+	Subscriber<M> *subscribe(const struct orb_metadata *meta,
 			      std::function<void(const M &)> callback,
 			      unsigned interval)
 	{
@@ -175,7 +175,7 @@ public:
 			_sub_min_interval = sub_px4;
 		}
 
-		return (Subscriber *)sub_px4;
+		return (Subscriber<M> *)sub_px4;
 	}
 
 	/**
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index cb15eeb58f..6b6d8e39e9 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -48,15 +48,30 @@
 
 namespace px4
 {
+
+/**
+ * Untemplated publisher base class
+ * */
+class PublisherBase
+{
+public:
+	PublisherBase() {};
+	~PublisherBase() {};
+
+};
+
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
-class Publisher
+class Publisher :
+	public PublisherBase
 {
 public:
 	/**
 	 * Construct Publisher by providing a ros::Publisher
 	 * @param ros_pub the ros publisher which will be used to perform the publications
 	 */
-	Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
+	Publisher(ros::Publisher ros_pub) :
+		PublisherBase(),
+		_ros_pub(ros_pub)
 	{}
 
 	~Publisher() {};
@@ -71,7 +86,8 @@ private:
 };
 #else
 class Publisher :
-	public uORB::PublicationNode
+	public uORB::PublicationNode,
+	public PublisherBase
 {
 public:
 	/**
@@ -81,7 +97,8 @@ public:
 	 */
 	Publisher(const struct orb_metadata *meta,
 		  List<uORB::PublicationNode *> *list) :
-		uORB::PublicationNode(meta, list)
+		uORB::PublicationNode(meta, list),
+		PublisherBase()
 	{}
 
 	~Publisher() {};
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 234b72f5b9..59c0ef70bd 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -50,14 +50,36 @@
 
 namespace px4
 {
+
+/**
+ * Untemplated subscriber base class
+ * */
+class SubscriberBase
+{
+public:
+	SubscriberBase() {};
+	~SubscriberBase() {};
+
+};
+
 /**
  * Subscriber class which is used by nodehandle
  */
-class Subscriber
+template<typename M>
+class Subscriber :
+	public SubscriberBase
 {
 public:
-	Subscriber() {};
+	Subscriber() :
+		SubscriberBase()
+	{};
 	~Subscriber() {};
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	virtual const M& get_msg() = 0;
 };
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
@@ -66,7 +88,7 @@ public:
  */
 template<typename M>
 class SubscriberROS :
-	public Subscriber
+	public Subscriber<M>
 {
 friend class NodeHandle;
 
@@ -75,7 +97,7 @@ public:
 	 * Construct Subscriber by providing a callback function
 	 */
 	SubscriberROS(std::function<void(const M &)> cbf) :
-		Subscriber(),
+		Subscriber<M>(),
 		_ros_sub(),
 		_cbf(cbf),
 		_msg_current()
@@ -85,7 +107,7 @@ public:
 	 * Construct Subscriber without a callback function
 	 */
 	SubscriberROS() :
-		Subscriber(),
+		Subscriber<M>(),
 		_ros_sub(),
 		_cbf(NULL),
 		_msg_current()
@@ -128,14 +150,14 @@ protected:
 
 };
 
-#else
+#else // Building for NuttX
 
 /**
  * Subscriber class that is templated with the uorb subscription message type
  */
 template<typename M>
 class SubscriberUORB :
-	public Subscriber,
+	public Subscriber<M>,
 	public uORB::Subscription<M>
 {
 public:
@@ -150,7 +172,7 @@ public:
 		      unsigned interval,
 		      std::function<void(const M &)> callback,
 		      List<uORB::SubscriptionNode *> *list) :
-		Subscriber(),
+		Subscriber<M>(),
 		uORB::Subscription<M>(meta, interval, list),
 		_callback(callback)
 		//XXX store callback

From 9c09a9e8d56b69797b64e5fa1f17d2bcf760e2bb Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 16:11:04 +0100
Subject: [PATCH 165/416] mc att control: prepare for ros integration, move
 class into spearate file

---
 src/modules/mc_att_control/mc_att_control.cpp | 403 ++++++++++++
 src/modules/mc_att_control/mc_att_control.h   | 174 ++++++
 .../mc_att_control/mc_att_control_base.cpp    |   3 +-
 .../mc_att_control/mc_att_control_base.h      |   2 +-
 .../mc_att_control/mc_att_control_main.cpp    | 585 +-----------------
 src/modules/mc_att_control/module.mk          |   1 +
 6 files changed, 610 insertions(+), 558 deletions(-)
 create mode 100644 src/modules/mc_att_control/mc_att_control.cpp
 create mode 100644 src/modules/mc_att_control/mc_att_control.h

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
new file mode 100644
index 0000000000..31cdd99e4d
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -0,0 +1,403 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#include "mc_att_control.h"
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+namespace mc_att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+}
+
+MulticopterAttitudeControl::MulticopterAttitudeControl() :
+	MulticopterAttitudeControlBase(),
+	_task_should_exit(false),
+	_control_task(-1),
+	_actuators_0_circuit_breaker_enabled(false),
+	/* subscriptions */
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+
+	/* publications */
+	_att_sp_pub(-1),
+	_v_rates_sp_pub(-1),
+	_actuators_0_pub(-1),
+	n(),
+
+	/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
+	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
+	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
+	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
+	_params_handles.yaw_p			=	param_find("MC_YAW_P");
+	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
+	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
+	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
+	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
+	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
+	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
+	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
+	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
+	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	/*
+	 * do subscriptions
+	 */
+	// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0);
+	// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0);
+	// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	PX4_SUBSCRIBE(n, vehicle_control_mode, 0);
+	// _params_sub = orb_subscribe(ORB_ID(parameter_update));
+	PX4_SUBSCRIBE(n, parameter_update, 0);
+	// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	PX4_SUBSCRIBE(n, manual_control_setpoint, 0);
+	// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	PX4_SUBSCRIBE(n, actuator_armed, 0);
+
+}
+
+MulticopterAttitudeControl::~MulticopterAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	// mc_att_control::g_control = nullptr;
+}
+
+int
+MulticopterAttitudeControl::parameters_update()
+{
+	float v;
+
+	/* roll gains */
+	param_get(_params_handles.roll_p, &v);
+	_params.att_p(0) = v;
+	param_get(_params_handles.roll_rate_p, &v);
+	_params.rate_p(0) = v;
+	param_get(_params_handles.roll_rate_i, &v);
+	_params.rate_i(0) = v;
+	param_get(_params_handles.roll_rate_d, &v);
+	_params.rate_d(0) = v;
+
+	/* pitch gains */
+	param_get(_params_handles.pitch_p, &v);
+	_params.att_p(1) = v;
+	param_get(_params_handles.pitch_rate_p, &v);
+	_params.rate_p(1) = v;
+	param_get(_params_handles.pitch_rate_i, &v);
+	_params.rate_i(1) = v;
+	param_get(_params_handles.pitch_rate_d, &v);
+	_params.rate_d(1) = v;
+
+	/* yaw gains */
+	param_get(_params_handles.yaw_p, &v);
+	_params.att_p(2) = v;
+	param_get(_params_handles.yaw_rate_p, &v);
+	_params.rate_p(2) = v;
+	param_get(_params_handles.yaw_rate_i, &v);
+	_params.rate_i(2) = v;
+	param_get(_params_handles.yaw_rate_d, &v);
+	_params.rate_d(2) = v;
+
+	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
+
+	/* manual control scale */
+	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
+	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);
+
+	/* acro control scale */
+	param_get(_params_handles.acro_roll_max, &v);
+	_params.acro_rate_max(0) = math::radians(v);
+	param_get(_params_handles.acro_pitch_max, &v);
+	_params.acro_rate_max(1) = math::radians(v);
+	param_get(_params_handles.acro_yaw_max, &v);
+	_params.acro_rate_max(2) = math::radians(v);
+
+	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
+
+	return OK;
+}
+
+void
+MulticopterAttitudeControl::parameter_update_poll()
+{
+	bool updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+
+void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
+
+	perf_begin(_loop_perf);
+
+	/* run controller on attitude changes */
+	static uint64_t last_run = 0;
+	float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+	last_run = hrt_absolute_time();
+
+	/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+	if (dt < 0.002f) {
+		dt = 0.002f;
+
+	} else if (dt > 0.02f) {
+		dt = 0.02f;
+	}
+
+	/* copy attitude topic */
+	orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+	/* check for updates in other topics */
+	parameter_update_poll();
+	vehicle_control_mode_poll();
+	arming_status_poll();
+	vehicle_manual_poll();
+
+	if (_v_control_mode.flag_control_attitude_enabled) {
+		control_attitude(dt);
+
+		/* publish the attitude setpoint if needed */
+		if (_publish_att_sp) {
+			_v_att_sp.timestamp = hrt_absolute_time();
+
+			if (_att_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+					    &_v_att_sp);
+
+			} else {
+				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+							    &_v_att_sp);
+			}
+		}
+
+		/* publish attitude rates setpoint */
+		_v_rates_sp.roll = _rates_sp(0);
+		_v_rates_sp.pitch = _rates_sp(1);
+		_v_rates_sp.yaw = _rates_sp(2);
+		_v_rates_sp.thrust = _thrust_sp;
+		_v_rates_sp.timestamp = hrt_absolute_time();
+
+		if (_v_rates_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+		} else {
+			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+		}
+
+	} else {
+		/* attitude controller disabled, poll rates setpoint topic */
+		if (_v_control_mode.flag_control_manual_enabled) {
+			/* manual rates control - ACRO mode */
+			_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
+						    _manual_control_sp.r).emult(_params.acro_rate_max);
+			_thrust_sp = _manual_control_sp.z;
+
+			/* reset yaw setpoint after ACRO */
+			_reset_yaw_sp = true;
+
+			/* publish attitude rates setpoint */
+			_v_rates_sp.roll = _rates_sp(0);
+			_v_rates_sp.pitch = _rates_sp(1);
+			_v_rates_sp.yaw = _rates_sp(2);
+			_v_rates_sp.thrust = _thrust_sp;
+			_v_rates_sp.timestamp = hrt_absolute_time();
+
+			if (_v_rates_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+			} else {
+				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+			}
+
+		} else {
+			/* attitude controller disabled, poll rates setpoint topic */
+			vehicle_rates_setpoint_poll();
+			_rates_sp(0) = _v_rates_sp.roll;
+			_rates_sp(1) = _v_rates_sp.pitch;
+			_rates_sp(2) = _v_rates_sp.yaw;
+			_thrust_sp = _v_rates_sp.thrust;
+		}
+	}
+
+	if (_v_control_mode.flag_control_rates_enabled) {
+		control_attitude_rates(dt);
+
+		/* publish actuator controls */
+		_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.timestamp = hrt_absolute_time();
+
+		if (!_actuators_0_circuit_breaker_enabled) {
+			if (_actuators_0_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+
+			} else {
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			}
+		}
+	}
+}
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
new file mode 100644
index 0000000000..48df887714
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control.h
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <px4.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include "mc_att_control_base.h"
+
+class MulticopterAttitudeControl :
+	public MulticopterAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControl();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~MulticopterAttitudeControl();
+
+	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+
+	void spin() { n.spin(); }
+
+private:
+	bool		_task_should_exit;			/**< if true, sensor task should exit */
+	int		_control_task;				/**< task handle for sensor task */
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
+
+	int		_v_att_sub;				/**< vehicle attitude subscription */
+	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
+	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
+	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
+	int		_params_sub;			/**< parameter updates subscription */
+	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
+	int		_armed_sub;				/**< arming status subscription */
+
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
+	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+
+	px4::NodeHandle n;
+
+	struct {
+		param_t roll_p;
+		param_t roll_rate_p;
+		param_t roll_rate_i;
+		param_t roll_rate_d;
+		param_t pitch_p;
+		param_t pitch_rate_p;
+		param_t pitch_rate_i;
+		param_t pitch_rate_d;
+		param_t yaw_p;
+		param_t yaw_rate_p;
+		param_t yaw_rate_i;
+		param_t yaw_rate_d;
+		param_t yaw_ff;
+		param_t yaw_rate_max;
+
+		param_t man_roll_max;
+		param_t man_pitch_max;
+		param_t man_yaw_max;
+		param_t acro_roll_max;
+		param_t acro_pitch_max;
+		param_t acro_yaw_max;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	perf_counter_t _loop_perf; /**< loop performance counter */
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Check for parameter update and handle it.
+	 */
+	void		parameter_update_poll();
+
+	/**
+	 * Check for changes in vehicle control mode.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for changes in manual inputs.
+	 */
+	void		vehicle_manual_poll();
+
+	/**
+	 * Check for attitude setpoint updates.
+	 */
+	void		vehicle_attitude_setpoint_poll();
+
+	/**
+	 * Check for rates setpoint updates.
+	 */
+	void		vehicle_rates_setpoint_poll();
+
+	/**
+	 * Check for arming status updates.
+	 */
+	void		arming_status_poll();
+
+};
+
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index f6b3268b59..d0ab1bfbf8 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -32,7 +32,7 @@
 /**
  * @file mc_att_control_base.cpp
  *
- * MC Attitude Controller
+ * MC Attitude Controller : Control and math code
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -46,6 +46,7 @@
 #include "mc_att_control_base.h"
 #include <geo/geo.h>
 #include <math.h>
+#include <lib/mathlib/mathlib.h>
 
 #ifdef CONFIG_ARCH_ARM
 #else
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index f81a0d7721..54a445e0df 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -35,7 +35,7 @@
 /**
  * @file mc_att_control_base.h
  *
- * MC Attitude Controller
+ * MC Attitude Controller : Control and math code
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 1103191e7f..a1dca8a8c9 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -52,548 +52,33 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
-#include <px4.h>
-#include <stdlib.h>
 #include <string.h>
-#include <unistd.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/circuit_breaker.h>
-#include <lib/mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-
-#include "mc_att_control_base.h"
+#include <cstdlib>
+#include "mc_att_control.h"
 
 static bool thread_running = false;     /**< Deamon status flag */
 static int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
 
 using namespace px4;
 
+PX4_MAIN_FUNCTION(mc_att_control);
+void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
+		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
+}
+
+
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 /**
  * Multicopter attitude control app start / stop handling function
  *
  * @ingroup apps
  */
 
-PX4_MAIN_FUNCTION(mc_att_control);
-
-int mc_attitude_thread_main(int argc, char *argv[]);
-
-#define YAW_DEADZONE	0.05f
-#define MIN_TAKEOFF_THRUST    0.2f
-#define RATES_I_LIMIT	0.3f
-
-void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
-		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
-}
-
-
-namespace px4
-{
-bool task_should_exit = false;
-}
-
-// PX4_MAIN_FUNCTION(mc_att_control) { px4::init(argc, argv, "listener");
-
-	// px4::NodeHandle n;
-
-	// PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000);
-
-	/**
-	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
-	 * callbacks will be called from within this thread (the main one).  px4::spin()
-	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
-	 */
-	// n.spin();
-	// PX4_INFO("finished, returning");
-
-	// return 0;
-// }
-
-class MulticopterAttitudeControl :
-	public MulticopterAttitudeControlBase
-{
-public:
-	/**
-	 * Constructor
-	 */
-	MulticopterAttitudeControl();
-
-	/**
-	 * Destructor, also kills the sensors task.
-	 */
-	~MulticopterAttitudeControl();
-
-	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
-
-	void spin() { n.spin(); }
-
-private:
-	bool		_task_should_exit;			/**< if true, sensor task should exit */
-	int		_control_task;				/**< task handle for sensor task */
-	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
-
-
-	int		_v_att_sub;				/**< vehicle attitude subscription */
-	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
-	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
-	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
-	int		_params_sub;			/**< parameter updates subscription */
-	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
-	int		_armed_sub;				/**< arming status subscription */
-
-	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
-	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
-	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
-
-	px4::NodeHandle n;
-
-	struct {
-		param_t roll_p;
-		param_t roll_rate_p;
-		param_t roll_rate_i;
-		param_t roll_rate_d;
-		param_t pitch_p;
-		param_t pitch_rate_p;
-		param_t pitch_rate_i;
-		param_t pitch_rate_d;
-		param_t yaw_p;
-		param_t yaw_rate_p;
-		param_t yaw_rate_i;
-		param_t yaw_rate_d;
-		param_t yaw_ff;
-		param_t yaw_rate_max;
-
-		param_t man_roll_max;
-		param_t man_pitch_max;
-		param_t man_yaw_max;
-		param_t acro_roll_max;
-		param_t acro_pitch_max;
-		param_t acro_yaw_max;
-	}		_params_handles;		/**< handles for interesting parameters */
-
-	perf_counter_t _loop_perf; /**< loop performance counter */
-
-	/**
-	 * Update our local parameter cache.
-	 */
-	int			parameters_update();
-
-	/**
-	 * Check for parameter update and handle it.
-	 */
-	void		parameter_update_poll();
-
-	/**
-	 * Check for changes in vehicle control mode.
-	 */
-	void		vehicle_control_mode_poll();
-
-	/**
-	 * Check for changes in manual inputs.
-	 */
-	void		vehicle_manual_poll();
-
-	/**
-	 * Check for attitude setpoint updates.
-	 */
-	void		vehicle_attitude_setpoint_poll();
-
-	/**
-	 * Check for rates setpoint updates.
-	 */
-	void		vehicle_rates_setpoint_poll();
-
-	/**
-	 * Check for arming status updates.
-	 */
-	void		arming_status_poll();
-
-};
-
-namespace mc_att_control
-{
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-}
-
-MulticopterAttitudeControl::MulticopterAttitudeControl() :
-	MulticopterAttitudeControlBase(),
-	_task_should_exit(false),
-	_control_task(-1),
-	_actuators_0_circuit_breaker_enabled(false),
-	/* subscriptions */
-	_v_att_sub(-1),
-	_v_att_sp_sub(-1),
-	_v_control_mode_sub(-1),
-	_params_sub(-1),
-	_manual_control_sp_sub(-1),
-	_armed_sub(-1),
-
-	/* publications */
-	_att_sp_pub(-1),
-	_v_rates_sp_pub(-1),
-	_actuators_0_pub(-1),
-	n(),
-
-	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
-
-{
-	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
-	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
-	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
-	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
-	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
-	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
-	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
-	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
-	_params_handles.yaw_p			=	param_find("MC_YAW_P");
-	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
-	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
-	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
-	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
-	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
-	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
-	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
-	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
-	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
-	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
-
-	/* fetch initial parameter values */
-	parameters_update();
-
-	/*
-	 * do subscriptions
-	 */
-	// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
-	// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0);
-	// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0);
-	// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	PX4_SUBSCRIBE(n, vehicle_control_mode, 0);
-	// _params_sub = orb_subscribe(ORB_ID(parameter_update));
-	PX4_SUBSCRIBE(n, parameter_update, 0);
-	// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	PX4_SUBSCRIBE(n, manual_control_setpoint, 0);
-	// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	PX4_SUBSCRIBE(n, actuator_armed, 0);
-
-}
-
-MulticopterAttitudeControl::~MulticopterAttitudeControl()
-{
-	if (_control_task != -1) {
-		/* task wakes up every 100ms or so at the longest */
-		_task_should_exit = true;
-
-		/* wait for a second for the task to quit at our request */
-		unsigned i = 0;
-
-		do {
-			/* wait 20ms */
-			usleep(20000);
-
-			/* if we have given up, kill it */
-			if (++i > 50) {
-				task_delete(_control_task);
-				break;
-			}
-		} while (_control_task != -1);
-	}
-
-	// mc_att_control::g_control = nullptr;
-}
-
-int
-MulticopterAttitudeControl::parameters_update()
-{
-	float v;
-
-	/* roll gains */
-	param_get(_params_handles.roll_p, &v);
-	_params.att_p(0) = v;
-	param_get(_params_handles.roll_rate_p, &v);
-	_params.rate_p(0) = v;
-	param_get(_params_handles.roll_rate_i, &v);
-	_params.rate_i(0) = v;
-	param_get(_params_handles.roll_rate_d, &v);
-	_params.rate_d(0) = v;
-
-	/* pitch gains */
-	param_get(_params_handles.pitch_p, &v);
-	_params.att_p(1) = v;
-	param_get(_params_handles.pitch_rate_p, &v);
-	_params.rate_p(1) = v;
-	param_get(_params_handles.pitch_rate_i, &v);
-	_params.rate_i(1) = v;
-	param_get(_params_handles.pitch_rate_d, &v);
-	_params.rate_d(1) = v;
-
-	/* yaw gains */
-	param_get(_params_handles.yaw_p, &v);
-	_params.att_p(2) = v;
-	param_get(_params_handles.yaw_rate_p, &v);
-	_params.rate_p(2) = v;
-	param_get(_params_handles.yaw_rate_i, &v);
-	_params.rate_i(2) = v;
-	param_get(_params_handles.yaw_rate_d, &v);
-	_params.rate_d(2) = v;
-
-	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
-	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
-	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
-
-	/* manual control scale */
-	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
-	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
-	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
-	_params.man_roll_max = math::radians(_params.man_roll_max);
-	_params.man_pitch_max = math::radians(_params.man_pitch_max);
-	_params.man_yaw_max = math::radians(_params.man_yaw_max);
-
-	/* acro control scale */
-	param_get(_params_handles.acro_roll_max, &v);
-	_params.acro_rate_max(0) = math::radians(v);
-	param_get(_params_handles.acro_pitch_max, &v);
-	_params.acro_rate_max(1) = math::radians(v);
-	param_get(_params_handles.acro_yaw_max, &v);
-	_params.acro_rate_max(2) = math::radians(v);
-
-	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
-
-	return OK;
-}
-
-void
-MulticopterAttitudeControl::parameter_update_poll()
-{
-	bool updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_params_sub, &updated);
-
-	if (updated) {
-		struct parameter_update_s param_update;
-		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
-		parameters_update();
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_control_mode_poll()
-{
-	bool updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_v_control_mode_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_manual_poll()
-{
-	bool updated;
-
-	/* get pilots inputs */
-	orb_check(_manual_control_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_att_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_rates_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::arming_status_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_armed_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-	}
-}
-
-// void
-// MulticopterAttitudeControl::task_main()
-// {
-
-
-	// [> wakeup source: vehicle attitude <]
-	// struct pollfd fds[1];
-
-	// fds[0].fd = _v_att_sub;
-	// fds[0].events = POLLIN;
-
-	// while (!_task_should_exit) {
-
-
-		// perf_end(_loop_perf);
-	// }
-
-	// warnx("exit");
-
-	// _control_task = -1;
-	// _exit(0);
-// }
-
-void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
-
-	perf_begin(_loop_perf);
-
-	/* run controller on attitude changes */
-	static uint64_t last_run = 0;
-	float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
-
-	/* guard against too small (< 2ms) and too large (> 20ms) dt's */
-	if (dt < 0.002f) {
-		dt = 0.002f;
-
-	} else if (dt > 0.02f) {
-		dt = 0.02f;
-	}
-
-	/* copy attitude topic */
-	orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
-
-	/* check for updates in other topics */
-	parameter_update_poll();
-	vehicle_control_mode_poll();
-	arming_status_poll();
-	vehicle_manual_poll();
-
-	if (_v_control_mode.flag_control_attitude_enabled) {
-		control_attitude(dt);
-
-		/* publish the attitude setpoint if needed */
-		if (_publish_att_sp) {
-			_v_att_sp.timestamp = hrt_absolute_time();
-
-			if (_att_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-					    &_v_att_sp);
-
-			} else {
-				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-							    &_v_att_sp);
-			}
-		}
-
-		/* publish attitude rates setpoint */
-		_v_rates_sp.roll = _rates_sp(0);
-		_v_rates_sp.pitch = _rates_sp(1);
-		_v_rates_sp.yaw = _rates_sp(2);
-		_v_rates_sp.thrust = _thrust_sp;
-		_v_rates_sp.timestamp = hrt_absolute_time();
-
-		if (_v_rates_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-		} else {
-			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-		}
-
-	} else {
-		/* attitude controller disabled, poll rates setpoint topic */
-		if (_v_control_mode.flag_control_manual_enabled) {
-			/* manual rates control - ACRO mode */
-			_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
-						    _manual_control_sp.r).emult(_params.acro_rate_max);
-			_thrust_sp = _manual_control_sp.z;
-
-			/* reset yaw setpoint after ACRO */
-			_reset_yaw_sp = true;
-
-			/* publish attitude rates setpoint */
-			_v_rates_sp.roll = _rates_sp(0);
-			_v_rates_sp.pitch = _rates_sp(1);
-			_v_rates_sp.yaw = _rates_sp(2);
-			_v_rates_sp.thrust = _thrust_sp;
-			_v_rates_sp.timestamp = hrt_absolute_time();
-
-			if (_v_rates_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-			} else {
-				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-			}
-
-		} else {
-			/* attitude controller disabled, poll rates setpoint topic */
-			vehicle_rates_setpoint_poll();
-			_rates_sp(0) = _v_rates_sp.roll;
-			_rates_sp(1) = _v_rates_sp.pitch;
-			_rates_sp(2) = _v_rates_sp.yaw;
-			_thrust_sp = _v_rates_sp.thrust;
-		}
-	}
-
-	if (_v_control_mode.flag_control_rates_enabled) {
-		control_attitude_rates(dt);
-
-		/* publish actuator controls */
-		_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-		_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-		_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-		_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
-		_actuators.timestamp = hrt_absolute_time();
-
-		if (!_actuators_0_circuit_breaker_enabled) {
-			if (_actuators_0_pub > 0) {
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
-
-			} else {
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
-			}
-		}
-	}
-}
-
-PX4_MAIN_FUNCTION(mc_att_control)
+extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
 {
 	px4::init(argc, argv, "mc_att_control");
 
@@ -615,54 +100,42 @@ PX4_MAIN_FUNCTION(mc_att_control)
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       2000,
-				       mc_attitude_thread_main,
+				       mc_att_control_task_main,
 					(argv) ? (const char **)&argv[2] : (const char **)NULL);
 
 		exit(0);
 	}
 
-	// if (!strcmp(argv[1], "stop")) {
-		// if (mc_att_control::g_control == nullptr) {
-			// errx(1, "not running");
-		// }
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
 
-		// delete mc_att_control::g_control;
-		// mc_att_control::g_control = nullptr;
-		// exit(0);
-	// }
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
 
-	// if (!strcmp(argv[1], "status")) {
-		// if (mc_att_control::g_control) {
-			// errx(0, "running");
+		} else {
+			warnx("not started");
+		}
 
-		// } else {
-			// errx(1, "not running");
-		// }
-	// }
+		exit(0);
+	}
 
 	warnx("unrecognized command");
 	return 1;
 }
+#endif
 
-int mc_attitude_thread_main(int argc, char *argv[])
+PX4_MAIN_FUNCTION(mc_att_control)
 {
-
 	warnx("starting");
-
 	MulticopterAttitudeControl attctl;
-
 	thread_running = true;
-
 	attctl.spin();
 
-	// while (!task_should_exit) {
-		// attctl.update();
-	// }
-
 	warnx("exiting.");
-
 	thread_running = false;
-
 	return 0;
 }
 
diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk
index ecd251d45c..29254f5bb6 100644
--- a/src/modules/mc_att_control/module.mk
+++ b/src/modules/mc_att_control/module.mk
@@ -38,5 +38,6 @@
 MODULE_COMMAND	= mc_att_control
 
 SRCS		= mc_att_control_main.cpp \
+		  mc_att_control.cpp \
 		  mc_att_control_base.cpp \
 		  mc_att_control_params.c

From ce73a816027fb394d23ef36d6d2c71774b231717 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 16:41:04 +0100
Subject: [PATCH 166/416] WIP remove unnecessary functions

---
 src/modules/mc_att_control/mc_att_control.cpp | 89 +------------------
 src/modules/mc_att_control/mc_att_control.h   | 31 -------
 .../mc_att_control/mc_att_control_base.cpp    |  4 +-
 .../mc_att_control/mc_att_control_base.h      |  2 -
 4 files changed, 3 insertions(+), 123 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 31cdd99e4d..0028127417 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -211,84 +211,6 @@ MulticopterAttitudeControl::parameters_update()
 	return OK;
 }
 
-void
-MulticopterAttitudeControl::parameter_update_poll()
-{
-	bool updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_params_sub, &updated);
-
-	if (updated) {
-		struct parameter_update_s param_update;
-		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
-		parameters_update();
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_control_mode_poll()
-{
-	bool updated;
-
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_v_control_mode_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_manual_poll()
-{
-	bool updated;
-
-	/* get pilots inputs */
-	orb_check(_manual_control_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_att_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_rates_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
-	}
-}
-
-void
-MulticopterAttitudeControl::arming_status_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_armed_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-	}
-}
-
-
 void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
 
 	perf_begin(_loop_perf);
@@ -306,15 +228,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		dt = 0.02f;
 	}
 
-	/* copy attitude topic */
-	orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
-
-	/* check for updates in other topics */
-	parameter_update_poll();
-	vehicle_control_mode_poll();
-	arming_status_poll();
-	vehicle_manual_poll();
-
 	if (_v_control_mode.flag_control_attitude_enabled) {
 		control_attitude(dt);
 
@@ -373,7 +286,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 		} else {
 			/* attitude controller disabled, poll rates setpoint topic */
-			vehicle_rates_setpoint_poll();
+			//XXX vehicle_rates_setpoint_poll();
 			_rates_sp(0) = _v_rates_sp.roll;
 			_rates_sp(1) = _v_rates_sp.pitch;
 			_rates_sp(2) = _v_rates_sp.yaw;
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 48df887714..b2ec3083f6 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -139,36 +139,5 @@ private:
 	 * Update our local parameter cache.
 	 */
 	int			parameters_update();
-
-	/**
-	 * Check for parameter update and handle it.
-	 */
-	void		parameter_update_poll();
-
-	/**
-	 * Check for changes in vehicle control mode.
-	 */
-	void		vehicle_control_mode_poll();
-
-	/**
-	 * Check for changes in manual inputs.
-	 */
-	void		vehicle_manual_poll();
-
-	/**
-	 * Check for attitude setpoint updates.
-	 */
-	void		vehicle_attitude_setpoint_poll();
-
-	/**
-	 * Check for rates setpoint updates.
-	 */
-	void		vehicle_rates_setpoint_poll();
-
-	/**
-	 * Check for arming status updates.
-	 */
-	void		arming_status_poll();
-
 };
 
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index d0ab1bfbf8..0a05be9245 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -101,7 +101,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		if (_v_control_mode.flag_control_velocity_enabled
 		    || _v_control_mode.flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			vehicle_attitude_setpoint_poll();
+			//XXX vehicle_attitude_setpoint_poll();
 		}
 
 		if (!_v_control_mode.flag_control_climb_rate_enabled) {
@@ -160,7 +160,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		vehicle_attitude_setpoint_poll();
+		//XXX vehicle_attitude_setpoint_poll();
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 54a445e0df..0783bb71ef 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -120,8 +120,6 @@ protected:
 
 	bool _publish_att_sp;
 
-	virtual void vehicle_attitude_setpoint_poll() = 0;
-
 };
 
 #endif /* MC_ATT_CONTROL_BASE_H_ */

From 9d0ecc77bb1c3d1c9b5cf6bc5d517e45d9f20f97 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 17:05:12 +0100
Subject: [PATCH 167/416] mc att: convert param definitions to be compatible
 with multiplatform build

---
 .../mc_att_control/mc_att_control_params.c    | 41 ++++++------
 .../mc_att_control/mc_att_control_params.h    | 65 +++++++++++++++++++
 2 files changed, 86 insertions(+), 20 deletions(-)
 create mode 100644 src/modules/mc_att_control/mc_att_control_params.h

diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index 819847b409..bbbfd829bb 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -40,6 +40,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+#include "mc_att_control_params.h""
 #include <systemlib/param/param.h>
 
 /**
@@ -50,7 +51,7 @@
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
 
 /**
  * Roll rate P gain
@@ -60,7 +61,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
 
 /**
  * Roll rate I gain
@@ -70,7 +71,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
 
 /**
  * Roll rate D gain
@@ -80,7 +81,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
 
 /**
  * Pitch P gain
@@ -91,7 +92,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
 
 /**
  * Pitch rate P gain
@@ -101,7 +102,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
 
 /**
  * Pitch rate I gain
@@ -111,7 +112,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
 
 /**
  * Pitch rate D gain
@@ -121,7 +122,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
 
 /**
  * Yaw P gain
@@ -132,7 +133,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
 
 /**
  * Yaw rate P gain
@@ -142,7 +143,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
 
 /**
  * Yaw rate I gain
@@ -152,7 +153,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
 
 /**
  * Yaw rate D gain
@@ -162,7 +163,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
 
 /**
  * Yaw feed forward
@@ -173,7 +174,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
  * @max 1.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
 
 /**
  * Max yaw rate
@@ -185,7 +186,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
 
 /**
  * Max manual roll
@@ -195,7 +196,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
 
 /**
  * Max manual pitch
@@ -205,7 +206,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
 
 /**
  * Max manual yaw rate
@@ -214,7 +215,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
 
 /**
  * Max acro roll rate
@@ -224,7 +225,7 @@ PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
 
 /**
  * Max acro pitch rate
@@ -234,7 +235,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
 
 /**
  * Max acro yaw rate
@@ -243,4 +244,4 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
diff --git a/src/modules/mc_att_control/mc_att_control_params.h b/src/modules/mc_att_control/mc_att_control_params.h
new file mode 100644
index 0000000000..59dd5240f6
--- /dev/null
+++ b/src/modules/mc_att_control/mc_att_control_params.h
@@ -0,0 +1,65 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_params.h
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define PARAM_MC_ROLL_P_DEFAULT 6.0f
+#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
+#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
+#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
+#define PARAM_MC_PITCH_P_DEFAULT 6.0f
+#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
+#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
+#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
+#define PARAM_MC_YAW_P_DEFAULT 2.0f
+#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
+#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
+#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
+#define PARAM_MC_YAW_FF_DEFAULT 0.5f
+#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
+#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f
+#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
+#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
+#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f

From b755312a1ec3e1f73827ac2b581a597491601140 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 17:12:03 +0100
Subject: [PATCH 168/416] mc att: use PX4 macros to init and get params

---
 src/modules/mc_att_control/mc_att_control.cpp | 80 +++++++++----------
 src/modules/mc_att_control/mc_att_control.h   | 40 +++++-----
 2 files changed, 60 insertions(+), 60 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 0028127417..d20bf24e11 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -83,26 +83,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
-	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
-	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
-	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
-	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
-	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
-	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
-	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
-	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
-	_params_handles.yaw_p			=	param_find("MC_YAW_P");
-	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
-	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
-	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
-	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
-	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
-	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
-	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
-	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
-	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
-	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
+	_params_handles.roll_p			= 	PX4_PARAM_INIT("MC_ROLL_P");
+	_params_handles.roll_rate_p		= 	PX4_PARAM_INIT("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i		= 	PX4_PARAM_INIT("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d		= 	PX4_PARAM_INIT("MC_ROLLRATE_D");
+	_params_handles.pitch_p			= 	PX4_PARAM_INIT("MC_PITCH_P");
+	_params_handles.pitch_rate_p		= 	PX4_PARAM_INIT("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i		= 	PX4_PARAM_INIT("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d		= 	PX4_PARAM_INIT("MC_PITCHRATE_D");
+	_params_handles.yaw_p			=	PX4_PARAM_INIT("MC_YAW_P");
+	_params_handles.yaw_rate_p		= 	PX4_PARAM_INIT("MC_YAWRATE_P");
+	_params_handles.yaw_rate_i		= 	PX4_PARAM_INIT("MC_YAWRATE_I");
+	_params_handles.yaw_rate_d		= 	PX4_PARAM_INIT("MC_YAWRATE_D");
+	_params_handles.yaw_ff			= 	PX4_PARAM_INIT("MC_YAW_FF");
+	_params_handles.yaw_rate_max		= 	PX4_PARAM_INIT("MC_YAWRATE_MAX");
+	_params_handles.man_roll_max		= 	PX4_PARAM_INIT("MC_MAN_R_MAX");
+	_params_handles.man_pitch_max		= 	PX4_PARAM_INIT("MC_MAN_P_MAX");
+	_params_handles.man_yaw_max		= 	PX4_PARAM_INIT("MC_MAN_Y_MAX");
+	_params_handles.acro_roll_max		= 	PX4_PARAM_INIT("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT("MC_ACRO_Y_MAX");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -157,53 +157,53 @@ MulticopterAttitudeControl::parameters_update()
 	float v;
 
 	/* roll gains */
-	param_get(_params_handles.roll_p, &v);
+	PX4_PARAM_GET(_params_handles.roll_p, &v);
 	_params.att_p(0) = v;
-	param_get(_params_handles.roll_rate_p, &v);
+	PX4_PARAM_GET(_params_handles.roll_rate_p, &v);
 	_params.rate_p(0) = v;
-	param_get(_params_handles.roll_rate_i, &v);
+	PX4_PARAM_GET(_params_handles.roll_rate_i, &v);
 	_params.rate_i(0) = v;
-	param_get(_params_handles.roll_rate_d, &v);
+	PX4_PARAM_GET(_params_handles.roll_rate_d, &v);
 	_params.rate_d(0) = v;
 
 	/* pitch gains */
-	param_get(_params_handles.pitch_p, &v);
+	PX4_PARAM_GET(_params_handles.pitch_p, &v);
 	_params.att_p(1) = v;
-	param_get(_params_handles.pitch_rate_p, &v);
+	PX4_PARAM_GET(_params_handles.pitch_rate_p, &v);
 	_params.rate_p(1) = v;
-	param_get(_params_handles.pitch_rate_i, &v);
+	PX4_PARAM_GET(_params_handles.pitch_rate_i, &v);
 	_params.rate_i(1) = v;
-	param_get(_params_handles.pitch_rate_d, &v);
+	PX4_PARAM_GET(_params_handles.pitch_rate_d, &v);
 	_params.rate_d(1) = v;
 
 	/* yaw gains */
-	param_get(_params_handles.yaw_p, &v);
+	PX4_PARAM_GET(_params_handles.yaw_p, &v);
 	_params.att_p(2) = v;
-	param_get(_params_handles.yaw_rate_p, &v);
+	PX4_PARAM_GET(_params_handles.yaw_rate_p, &v);
 	_params.rate_p(2) = v;
-	param_get(_params_handles.yaw_rate_i, &v);
+	PX4_PARAM_GET(_params_handles.yaw_rate_i, &v);
 	_params.rate_i(2) = v;
-	param_get(_params_handles.yaw_rate_d, &v);
+	PX4_PARAM_GET(_params_handles.yaw_rate_d, &v);
 	_params.rate_d(2) = v;
 
-	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
-	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff);
+	PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
 	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
 
 	/* manual control scale */
-	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
-	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
-	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+	PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max);
+	PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max);
 	_params.man_roll_max = math::radians(_params.man_roll_max);
 	_params.man_pitch_max = math::radians(_params.man_pitch_max);
 	_params.man_yaw_max = math::radians(_params.man_yaw_max);
 
 	/* acro control scale */
-	param_get(_params_handles.acro_roll_max, &v);
+	PX4_PARAM_GET(_params_handles.acro_roll_max, &v);
 	_params.acro_rate_max(0) = math::radians(v);
-	param_get(_params_handles.acro_pitch_max, &v);
+	PX4_PARAM_GET(_params_handles.acro_pitch_max, &v);
 	_params.acro_rate_max(1) = math::radians(v);
-	param_get(_params_handles.acro_yaw_max, &v);
+	PX4_PARAM_GET(_params_handles.acro_yaw_max, &v);
 	_params.acro_rate_max(2) = math::radians(v);
 
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index b2ec3083f6..4e1dd0b577 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -110,27 +110,27 @@ private:
 	px4::NodeHandle n;
 
 	struct {
-		param_t roll_p;
-		param_t roll_rate_p;
-		param_t roll_rate_i;
-		param_t roll_rate_d;
-		param_t pitch_p;
-		param_t pitch_rate_p;
-		param_t pitch_rate_i;
-		param_t pitch_rate_d;
-		param_t yaw_p;
-		param_t yaw_rate_p;
-		param_t yaw_rate_i;
-		param_t yaw_rate_d;
-		param_t yaw_ff;
-		param_t yaw_rate_max;
+		px4_param_t roll_p;
+		px4_param_t roll_rate_p;
+		px4_param_t roll_rate_i;
+		px4_param_t roll_rate_d;
+		px4_param_t pitch_p;
+		px4_param_t pitch_rate_p;
+		px4_param_t pitch_rate_i;
+		px4_param_t pitch_rate_d;
+		px4_param_t yaw_p;
+		px4_param_t yaw_rate_p;
+		px4_param_t yaw_rate_i;
+		px4_param_t yaw_rate_d;
+		px4_param_t yaw_ff;
+		px4_param_t yaw_rate_max;
 
-		param_t man_roll_max;
-		param_t man_pitch_max;
-		param_t man_yaw_max;
-		param_t acro_roll_max;
-		param_t acro_pitch_max;
-		param_t acro_yaw_max;
+		px4_param_t man_roll_max;
+		px4_param_t man_pitch_max;
+		px4_param_t man_yaw_max;
+		px4_param_t acro_roll_max;
+		px4_param_t acro_pitch_max;
+		px4_param_t acro_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	perf_counter_t _loop_perf; /**< loop performance counter */

From ab35b6470cd9ff8fe9fd9a1bf1f6617801189e83 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 11 Dec 2014 19:02:14 +0100
Subject: [PATCH 169/416] mc att: use multiplatform publisher

---
 src/modules/mc_att_control/mc_att_control.cpp | 48 +++++++++----------
 src/modules/mc_att_control/mc_att_control.h   | 10 ++--
 src/modules/uORB/uORB.h                       |  1 +
 src/platforms/px4_defines.h                   |  2 +
 4 files changed, 31 insertions(+), 30 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index d20bf24e11..89daa8b484 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -74,10 +74,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_armed_sub(-1),
 
 	/* publications */
-	_att_sp_pub(-1),
-	_v_rates_sp_pub(-1),
-	_actuators_0_pub(-1),
-	n(),
+	_att_sp_pub(nullptr),
+	_v_rates_sp_pub(nullptr),
+	_actuators_0_pub(nullptr),
+	_n(),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -111,19 +111,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	 * do subscriptions
 	 */
 	// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
 	// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0);
+	PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
 	// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0);
+	PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
 	// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	PX4_SUBSCRIBE(n, vehicle_control_mode, 0);
+	PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
 	// _params_sub = orb_subscribe(ORB_ID(parameter_update));
-	PX4_SUBSCRIBE(n, parameter_update, 0);
+	PX4_SUBSCRIBE(_n, parameter_update, 0);
 	// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	PX4_SUBSCRIBE(n, manual_control_setpoint, 0);
+	PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
 	// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	PX4_SUBSCRIBE(n, actuator_armed, 0);
+	PX4_SUBSCRIBE(_n, actuator_armed, 0);
 
 }
 
@@ -235,13 +235,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		if (_publish_att_sp) {
 			_v_att_sp.timestamp = hrt_absolute_time();
 
-			if (_att_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
-					    &_v_att_sp);
+			if (_att_sp_pub != nullptr) {
+				_att_sp_pub->publish(_v_att_sp);
 
 			} else {
-				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
-							    &_v_att_sp);
+				_att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint);
 			}
 		}
 
@@ -252,11 +250,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		_v_rates_sp.thrust = _thrust_sp;
 		_v_rates_sp.timestamp = hrt_absolute_time();
 
-		if (_v_rates_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+		if (_v_rates_sp_pub != nullptr) {
+			_v_rates_sp_pub->publish(_v_rates_sp);
 
 		} else {
-			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+			_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
 		}
 
 	} else {
@@ -277,11 +275,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 			_v_rates_sp.thrust = _thrust_sp;
 			_v_rates_sp.timestamp = hrt_absolute_time();
 
-			if (_v_rates_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+			if (_v_rates_sp_pub != nullptr) {
+				_v_rates_sp_pub->publish(_v_rates_sp);
 
 			} else {
-				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
 			}
 
 		} else {
@@ -305,11 +303,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		_actuators.timestamp = hrt_absolute_time();
 
 		if (!_actuators_0_circuit_breaker_enabled) {
-			if (_actuators_0_pub > 0) {
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+			if (_actuators_0_pub != nullptr) {
+				_actuators_0_pub->publish(_actuators);
 
 			} else {
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
 			}
 		}
 	}
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 4e1dd0b577..229e8e672f 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -87,7 +87,7 @@ public:
 
 	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
 
-	void spin() { n.spin(); }
+	void spin() { _n.spin(); }
 
 private:
 	bool		_task_should_exit;			/**< if true, sensor task should exit */
@@ -103,11 +103,11 @@ private:
 	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
 	int		_armed_sub;				/**< arming status subscription */
 
-	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
-	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
-	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+	px4::Publisher *	_att_sp_pub;			/**< attitude setpoint publication */
+	px4::Publisher *	_v_rates_sp_pub;		/**< rate setpoint publication */
+	px4::Publisher *	_actuators_0_pub;		/**< attitude actuator controls publication */
 
-	px4::NodeHandle n;
+	px4::NodeHandle _n;
 
 	struct {
 		px4_param_t roll_p;
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 578ebf9ddc..0a99cce77d 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -267,5 +267,6 @@ ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
+typedef struct actuator_controls_s actuator_controls_0_s;
 
 #endif /* _UORB_UORB_H */
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 9bb190380e..a1bf9919e9 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -138,6 +138,8 @@ typedef param_t px4_param_t;
 
 #endif
 
+/* Defines for all platforms */
+
 /* Shortcut for subscribing to topics
  * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all
  */

From 59d26f5c30c9f70cd1e8bb7152ab07c96231d4e2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 12 Dec 2014 07:34:23 +0100
Subject: [PATCH 170/416] mc att: remove subscriber handles

---
 src/modules/mc_att_control/mc_att_control.cpp | 14 --------------
 src/modules/mc_att_control/mc_att_control.h   |  9 ---------
 2 files changed, 23 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 89daa8b484..bbda1f8aae 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -65,13 +65,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_task_should_exit(false),
 	_control_task(-1),
 	_actuators_0_circuit_breaker_enabled(false),
-	/* subscriptions */
-	_v_att_sub(-1),
-	_v_att_sp_sub(-1),
-	_v_control_mode_sub(-1),
-	_params_sub(-1),
-	_manual_control_sp_sub(-1),
-	_armed_sub(-1),
 
 	/* publications */
 	_att_sp_pub(nullptr),
@@ -110,19 +103,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	/*
 	 * do subscriptions
 	 */
-	// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
-	// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
-	// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 	PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
-	// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
-	// _params_sub = orb_subscribe(ORB_ID(parameter_update));
 	PX4_SUBSCRIBE(_n, parameter_update, 0);
-	// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
-	// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	PX4_SUBSCRIBE(_n, actuator_armed, 0);
 
 }
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 229e8e672f..24009a5e66 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -94,15 +94,6 @@ private:
 	int		_control_task;				/**< task handle for sensor task */
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
-
-	int		_v_att_sub;				/**< vehicle attitude subscription */
-	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
-	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
-	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
-	int		_params_sub;			/**< parameter updates subscription */
-	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
-	int		_armed_sub;				/**< arming status subscription */
-
 	px4::Publisher *	_att_sp_pub;			/**< attitude setpoint publication */
 	px4::Publisher *	_v_rates_sp_pub;		/**< rate setpoint publication */
 	px4::Publisher *	_actuators_0_pub;		/**< attitude actuator controls publication */

From c0d386bce0a2fd7d119dd8495d1ca68d985ae411 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 12 Dec 2014 10:15:33 +0100
Subject: [PATCH 171/416] mc att: use multiplatform subscriptions type

---
 .../subscriber/subscriber_example.cpp         |  2 +-
 src/modules/mc_att_control/mc_att_control.cpp | 12 ++---
 .../mc_att_control/mc_att_control_base.cpp    | 52 +++++++++----------
 .../mc_att_control/mc_att_control_base.h      | 15 +++---
 src/platforms/px4_subscriber.h                |  6 +--
 5 files changed, 44 insertions(+), 43 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 83ead4ba5a..3c80561caf 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -78,5 +78,5 @@ SubscriberExample::SubscriberExample() :
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
 			msg.timestamp_last_valid,
-			_sub_rc_chan->get_msg().timestamp_last_valid);
+			_sub_rc_chan->get().timestamp_last_valid);
 }
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index bbda1f8aae..59eaaec6dc 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -214,7 +214,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		dt = 0.02f;
 	}
 
-	if (_v_control_mode.flag_control_attitude_enabled) {
+	if (_v_control_mode->get().flag_control_attitude_enabled) {
 		control_attitude(dt);
 
 		/* publish the attitude setpoint if needed */
@@ -245,11 +245,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 	} else {
 		/* attitude controller disabled, poll rates setpoint topic */
-		if (_v_control_mode.flag_control_manual_enabled) {
+		if (_v_control_mode->get().flag_control_manual_enabled) {
 			/* manual rates control - ACRO mode */
-			_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
-						    _manual_control_sp.r).emult(_params.acro_rate_max);
-			_thrust_sp = _manual_control_sp.z;
+			_rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x,
+						    _manual_control_sp->get().r).emult(_params.acro_rate_max);
+			_thrust_sp = _manual_control_sp->get().z;
 
 			/* reset yaw setpoint after ACRO */
 			_reset_yaw_sp = true;
@@ -278,7 +278,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		}
 	}
 
-	if (_v_control_mode.flag_control_rates_enabled) {
+	if (_v_control_mode->get().flag_control_rates_enabled) {
 		control_attitude_rates(dt);
 
 		/* publish actuator controls */
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 0a05be9245..b9a57ce693 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -58,13 +58,13 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_publish_att_sp(false)
 
 {
-	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_actuators, 0, sizeof(_actuators));
-	memset(&_armed, 0, sizeof(_armed));
+	// memset(&_v_att, 0, sizeof(_v_att));
+	// memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	// memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	// memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	// memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	// memset(&_actuators, 0, sizeof(_actuators));
+	// memset(&_armed, 0, sizeof(_armed));
 
 	_params.att_p.zero();
 	_params.rate_p.zero();
@@ -95,22 +95,22 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	float yaw_sp_move_rate = 0.0f;
 	_publish_att_sp = false;
 
-	if (_v_control_mode.flag_control_manual_enabled) {
+	if (_v_control_mode->get().flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
 
-		if (_v_control_mode.flag_control_velocity_enabled
-		    || _v_control_mode.flag_control_climb_rate_enabled) {
+		if (_v_control_mode->get().flag_control_velocity_enabled
+		    || _v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
 			//XXX vehicle_attitude_setpoint_poll();
 		}
 
-		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+		if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.z;
+			_v_att_sp.thrust = _manual_control_sp->get().z;
 			_publish_att_sp = true;
 		}
 
-		if (!_armed.armed) {
+		if (!_armed->get().armed) {
 			/* reset yaw setpoint when disarmed */
 			_reset_yaw_sp = true;
 		}
@@ -124,17 +124,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(
 						     _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att->get().yaw);
 
 			if (yaw_offs < -yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
 			}
 
 			_v_att_sp.R_valid = false;
@@ -144,15 +144,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		/* reset yaw setpint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.yaw_body = _v_att->get().yaw;
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
 		}
 
-		if (!_v_control_mode.flag_control_velocity_enabled) {
+		if (!_v_control_mode->get().flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.x
+			_v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp->get().x
 					       * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
@@ -188,7 +188,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
-	R.set(_v_att.R);
+	R.set(_v_att->get().R);
 
 	/* all input data is ready, run controller itself */
 
@@ -269,15 +269,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed.armed) {
+	if (!_armed->get().armed) {
 		_rates_int.zero();
 	}
 
 	/* current body angular rates */
 	math::Vector < 3 > rates;
-	rates(0) = _v_att.rollspeed;
-	rates(1) = _v_att.pitchspeed;
-	rates(2) = _v_att.yawspeed;
+	rates(0) = _v_att->get().rollspeed;
+	rates(1) = _v_att->get().pitchspeed;
+	rates(2) = _v_att->get().yawspeed;
 
 	/* angular rates error */
 	math::Vector < 3 > rates_err = _rates_sp - rates;
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 0783bb71ef..05f89abe6c 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -86,13 +86,14 @@ public:
 	void set_actuator_controls();
 
 protected:
-	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
-	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
-	struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
-	struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
-	struct actuator_controls_s _actuators; /**< actuator controls */
-	struct actuator_armed_s _armed; /**< actuator arming status */
+	px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att;			    /**< vehicle attitude */
+	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;  /**< manual control setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
+	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
+
+	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp;	/**< vehicle attitude setpoint */
+	PX4_TOPIC_T(vehicle_rates_setpoint)	_v_rates_sp;	/**< vehicle rates setpoint */
+	PX4_TOPIC_T(actuator_controls)		_actuators;	/**< actuator controls */
 
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */
 	math::Vector<3> _rates_sp; /**< angular rates setpoint */
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 59c0ef70bd..7d8463cb52 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -79,7 +79,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	virtual const M& get_msg() = 0;
+	virtual const M& get() = 0;
 };
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
@@ -120,7 +120,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	const M& get_msg() { return _msg_current; }
+	const M& get() { return _msg_current; }
 
 protected:
 	/**
@@ -210,7 +210,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	const M& get_msg() { return uORB::Subscription<M>::getData(); }
+	const M& get() { return uORB::Subscription<M>::getData(); }
 
 protected:
 	std::function<void(const M &)> _callback;	/**< Callback handle,

From 75a870153792248b0e32243b30213d69c3d8d9db Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 12 Dec 2014 11:21:43 +0100
Subject: [PATCH 172/416] mc att: correctly handle topics which are
 simultaneously subscribed and published

---
 src/modules/mc_att_control/mc_att_control.cpp | 37 ++++++------
 .../mc_att_control/mc_att_control_base.cpp    | 57 +++++++++----------
 .../mc_att_control/mc_att_control_base.h      |  8 ++-
 3 files changed, 51 insertions(+), 51 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 59eaaec6dc..13bff9561b 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -219,10 +219,10 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 		/* publish the attitude setpoint if needed */
 		if (_publish_att_sp) {
-			_v_att_sp.timestamp = hrt_absolute_time();
+			_v_att_sp_mod.timestamp = hrt_absolute_time();
 
 			if (_att_sp_pub != nullptr) {
-				_att_sp_pub->publish(_v_att_sp);
+				_att_sp_pub->publish(_v_att_sp_mod);
 
 			} else {
 				_att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint);
@@ -230,15 +230,14 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		}
 
 		/* publish attitude rates setpoint */
-		_v_rates_sp.roll = _rates_sp(0);
-		_v_rates_sp.pitch = _rates_sp(1);
-		_v_rates_sp.yaw = _rates_sp(2);
-		_v_rates_sp.thrust = _thrust_sp;
-		_v_rates_sp.timestamp = hrt_absolute_time();
+		_v_rates_sp_mod.roll = _rates_sp(0);
+		_v_rates_sp_mod.pitch = _rates_sp(1);
+		_v_rates_sp_mod.yaw = _rates_sp(2);
+		_v_rates_sp_mod.thrust = _thrust_sp;
+		_v_rates_sp_mod.timestamp = hrt_absolute_time();
 
 		if (_v_rates_sp_pub != nullptr) {
-			_v_rates_sp_pub->publish(_v_rates_sp);
-
+			_v_rates_sp_pub->publish(_v_rates_sp_mod);
 		} else {
 			_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
 		}
@@ -255,14 +254,14 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 			_reset_yaw_sp = true;
 
 			/* publish attitude rates setpoint */
-			_v_rates_sp.roll = _rates_sp(0);
-			_v_rates_sp.pitch = _rates_sp(1);
-			_v_rates_sp.yaw = _rates_sp(2);
-			_v_rates_sp.thrust = _thrust_sp;
-			_v_rates_sp.timestamp = hrt_absolute_time();
+			_v_rates_sp_mod.roll = _rates_sp(0);
+			_v_rates_sp_mod.pitch = _rates_sp(1);
+			_v_rates_sp_mod.yaw = _rates_sp(2);
+			_v_rates_sp_mod.thrust = _thrust_sp;
+			_v_rates_sp_mod.timestamp = hrt_absolute_time();
 
 			if (_v_rates_sp_pub != nullptr) {
-				_v_rates_sp_pub->publish(_v_rates_sp);
+				_v_rates_sp_pub->publish(_v_rates_sp_mod);
 
 			} else {
 				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
@@ -271,10 +270,10 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		} else {
 			/* attitude controller disabled, poll rates setpoint topic */
 			//XXX vehicle_rates_setpoint_poll();
-			_rates_sp(0) = _v_rates_sp.roll;
-			_rates_sp(1) = _v_rates_sp.pitch;
-			_rates_sp(2) = _v_rates_sp.yaw;
-			_thrust_sp = _v_rates_sp.thrust;
+			_rates_sp(0) = _v_rates_sp->get().roll;
+			_rates_sp(1) = _v_rates_sp->get().pitch;
+			_rates_sp(2) = _v_rates_sp->get().yaw;
+			_thrust_sp = _v_rates_sp->get().thrust;
 		}
 	}
 
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index b9a57ce693..871f93ab88 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -58,13 +58,9 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_publish_att_sp(false)
 
 {
-	// memset(&_v_att, 0, sizeof(_v_att));
-	// memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	// memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	// memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	// memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	// memset(&_actuators, 0, sizeof(_actuators));
-	// memset(&_armed, 0, sizeof(_armed));
+	memset(&_v_rates_sp, 0, sizeof(_v_att_sp_mod));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod));
+	memset(&_actuators, 0, sizeof(_actuators));
 
 	_params.att_p.zero();
 	_params.rate_p.zero();
@@ -95,18 +91,19 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	float yaw_sp_move_rate = 0.0f;
 	_publish_att_sp = false;
 
+
 	if (_v_control_mode->get().flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
 
 		if (_v_control_mode->get().flag_control_velocity_enabled
 		    || _v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			//XXX vehicle_attitude_setpoint_poll();
+			_v_att_sp_mod = _v_att_sp->get();
 		}
 
 		if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp->get().z;
+			_v_att_sp_mod.thrust = _manual_control_sp->get().z;
 			_publish_att_sp = true;
 		}
 
@@ -116,7 +113,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		}
 
 		/* move yaw setpoint in all modes */
-		if (_v_att_sp.thrust < 0.1f) {
+		if (_v_att_sp_mod.thrust < 0.1f) {
 			// TODO
 			//if (_status.condition_landed) {
 			/* reset yaw setpoint if on ground */
@@ -125,65 +122,65 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		} else {
 			/* move yaw setpoint */
 			yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max;
-			_v_att_sp.yaw_body = _wrap_pi(
-						     _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			_v_att_sp_mod.yaw_body = _wrap_pi(
+						     _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt);
 			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att->get().yaw);
+			float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw);
 
 			if (yaw_offs < -yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
+				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
+				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
 			}
 
-			_v_att_sp.R_valid = false;
+			_v_att_sp_mod.R_valid = false;
 			_publish_att_sp = true;
 		}
 
 		/* reset yaw setpint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att->get().yaw;
-			_v_att_sp.R_valid = false;
+			_v_att_sp_mod.yaw_body = _v_att->get().yaw;
+			_v_att_sp_mod.R_valid = false;
 			_publish_att_sp = true;
 		}
 
 		if (!_v_control_mode->get().flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp->get().x
+			_v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
+			_v_att_sp_mod.pitch_body = -_manual_control_sp->get().x
 					       * _params.man_pitch_max;
-			_v_att_sp.R_valid = false;
+			_v_att_sp_mod.R_valid = false;
 			_publish_att_sp = true;
 		}
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		//XXX vehicle_attitude_setpoint_poll();
+		_v_att_sp_mod = _v_att_sp->get();
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
 	}
 
-	_thrust_sp = _v_att_sp.thrust;
+	_thrust_sp = _v_att_sp_mod.thrust;
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
 
-	if (_v_att_sp.R_valid) {
+	if (_v_att_sp_mod.R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0]);
+		R_sp.set(&_v_att_sp_mod.R_body[0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body,
-				_v_att_sp.yaw_body);
+		R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body,
+				_v_att_sp_mod.yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0],
-		       sizeof(_v_att_sp.R_body));
-		_v_att_sp.R_valid = true;
+		memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0],
+		       sizeof(_v_att_sp_mod.R_body));
+		_v_att_sp_mod.R_valid = true;
 	}
 
 	/* rotation matrix for current state */
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 05f89abe6c..2135b61d82 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -90,9 +90,13 @@ protected:
 	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;  /**< manual control setpoint */
 	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
 	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
+	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) * _v_att_sp;	/**< vehicle attitude setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) * _v_rates_sp;	/**< vehicle rates setpoint */
 
-	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp;	/**< vehicle attitude setpoint */
-	PX4_TOPIC_T(vehicle_rates_setpoint)	_v_rates_sp;	/**< vehicle rates setpoint */
+	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
+								  that gets published eventually */
+	PX4_TOPIC_T(vehicle_rates_setpoint)	_v_rates_sp_mod;	/**< vehicle rates setpoint
+								  that gets published eventually*/
 	PX4_TOPIC_T(actuator_controls)		_actuators;	/**< actuator controls */
 
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */

From f19b8e570c2631a6f75b25e9a8e5b007cdc6000d Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 15 Dec 2014 09:06:58 +0100
Subject: [PATCH 173/416] added file to make PX4 math functions compatible with
 eigen

---
 src/platforms/ros/eigen_math.h | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)
 create mode 100755 src/platforms/ros/eigen_math.h

diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h
new file mode 100755
index 0000000000..60fc9b93cd
--- /dev/null
+++ b/src/platforms/ros/eigen_math.h
@@ -0,0 +1,20 @@
+/*
+ * eigen_math.h
+ *
+ *  Created on: Aug 25, 2014
+ *      Author: roman
+ */
+
+#ifndef EIGEN_MATH_H_
+#define EIGEN_MATH_H_
+
+
+struct eigen_matrix_instance
+{
+	int numRows;
+	int numCols;
+	float *pData;
+};
+
+
+#endif /* EIGEN_MATH_H_ */

From 9980e4482146333340cc105b560bdbd26acb999f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 16 Dec 2014 08:22:58 +0100
Subject: [PATCH 174/416] moved msg files

---
 msg/{px4_msgs => }/actuator_armed.msg            | 0
 msg/{px4_msgs => }/actuator_controls.msg         | 0
 msg/actuator_controls_0.msg                      | 4 ++++
 msg/{px4_msgs => }/manual_control_setpoint.msg   | 0
 msg/{px4_msgs => }/parameter_update.msg          | 0
 msg/{px4_msgs => }/rc_channels.msg               | 0
 msg/templates/msg.h.template                     | 3 ++-
 msg/{px4_msgs => }/vehicle_attitude.msg          | 0
 msg/{px4_msgs => }/vehicle_attitude_setpoint.msg | 0
 msg/{px4_msgs => }/vehicle_control_mode.msg      | 0
 msg/{px4_msgs => }/vehicle_rates_setpoint.msg    | 0
 11 files changed, 6 insertions(+), 1 deletion(-)
 rename msg/{px4_msgs => }/actuator_armed.msg (100%)
 rename msg/{px4_msgs => }/actuator_controls.msg (100%)
 create mode 100644 msg/actuator_controls_0.msg
 rename msg/{px4_msgs => }/manual_control_setpoint.msg (100%)
 rename msg/{px4_msgs => }/parameter_update.msg (100%)
 rename msg/{px4_msgs => }/rc_channels.msg (100%)
 rename msg/{px4_msgs => }/vehicle_attitude.msg (100%)
 rename msg/{px4_msgs => }/vehicle_attitude_setpoint.msg (100%)
 rename msg/{px4_msgs => }/vehicle_control_mode.msg (100%)
 rename msg/{px4_msgs => }/vehicle_rates_setpoint.msg (100%)

diff --git a/msg/px4_msgs/actuator_armed.msg b/msg/actuator_armed.msg
similarity index 100%
rename from msg/px4_msgs/actuator_armed.msg
rename to msg/actuator_armed.msg
diff --git a/msg/px4_msgs/actuator_controls.msg b/msg/actuator_controls.msg
similarity index 100%
rename from msg/px4_msgs/actuator_controls.msg
rename to msg/actuator_controls.msg
diff --git a/msg/actuator_controls_0.msg b/msg/actuator_controls_0.msg
new file mode 100644
index 0000000000..743f20cdff
--- /dev/null
+++ b/msg/actuator_controls_0.msg
@@ -0,0 +1,4 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+float32[8] control
diff --git a/msg/px4_msgs/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg
similarity index 100%
rename from msg/px4_msgs/manual_control_setpoint.msg
rename to msg/manual_control_setpoint.msg
diff --git a/msg/px4_msgs/parameter_update.msg b/msg/parameter_update.msg
similarity index 100%
rename from msg/px4_msgs/parameter_update.msg
rename to msg/parameter_update.msg
diff --git a/msg/px4_msgs/rc_channels.msg b/msg/rc_channels.msg
similarity index 100%
rename from msg/px4_msgs/rc_channels.msg
rename to msg/rc_channels.msg
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index 5fce506b6b..19068a3a1e 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -104,7 +104,8 @@ type_map = {'int8': 'int8_t',
   'uint32': 'uint32_t',
   'uint64': 'uint64_t',
   'float32': 'float',
-  'bool': 'bool'}
+  'bool': 'bool',
+  'fence_vertex': 'fence_vertex'}
 
 # Function to print a standard ros type
 def print_field_def(field):
diff --git a/msg/px4_msgs/vehicle_attitude.msg b/msg/vehicle_attitude.msg
similarity index 100%
rename from msg/px4_msgs/vehicle_attitude.msg
rename to msg/vehicle_attitude.msg
diff --git a/msg/px4_msgs/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg
similarity index 100%
rename from msg/px4_msgs/vehicle_attitude_setpoint.msg
rename to msg/vehicle_attitude_setpoint.msg
diff --git a/msg/px4_msgs/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg
similarity index 100%
rename from msg/px4_msgs/vehicle_control_mode.msg
rename to msg/vehicle_control_mode.msg
diff --git a/msg/px4_msgs/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg
similarity index 100%
rename from msg/px4_msgs/vehicle_rates_setpoint.msg
rename to msg/vehicle_rates_setpoint.msg

From 9520983e08397c453af735d0ff0736cc007c2c45 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 16 Dec 2014 08:24:51 +0100
Subject: [PATCH 175/416] lots' of header juggling and small changes to make mc
 att control compile for NuttX and ROS

---
 CMakeLists.txt                                |  42 +++--
 Makefile                                      |   2 +-
 makefiles/config_px4fmu-v2_default.mk         |   4 +-
 package.xml                                   |   2 +
 src/include/px4.h                             |   2 +
 src/lib/geo/geo.h                             |   5 +-
 src/lib/mathlib/math/Matrix.hpp               |  12 +-
 src/lib/mathlib/math/Vector.hpp               |   2 +-
 src/modules/bottle_drop/bottle_drop.cpp       |   1 +
 src/modules/mc_att_control/mc_att_control.cpp |  75 +++-----
 src/modules/mc_att_control/mc_att_control.h   |   9 +-
 .../mc_att_control/mc_att_control_main.cpp    |   4 +-
 src/modules/systemlib/circuit_breaker.c       |  17 +-
 src/modules/systemlib/circuit_breaker.h       |   8 +-
 .../systemlib/circuit_breaker_params.h        |   7 +
 src/modules/systemlib/perf_counter.h          |   1 +
 src/platforms/px4_defines.h                   |  37 +++-
 src/platforms/px4_includes.h                  |  13 ++
 src/platforms/ros/circuit_breaker.cpp         |  56 ++++++
 src/platforms/ros/geo.cpp                     | 165 ++++++++++++++++++
 20 files changed, 376 insertions(+), 88 deletions(-)
 create mode 100644 src/modules/systemlib/circuit_breaker_params.h
 create mode 100644 src/platforms/ros/circuit_breaker.cpp
 create mode 100644 src/platforms/ros/geo.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 489467db7d..19a14f62af 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,9 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   message_generation
+  cmake_modules
 )
+find_package(Eigen REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -48,9 +50,17 @@ find_package(catkin REQUIRED COMPONENTS
 ## Generate messages in the 'msg' folder
 add_message_files(
   FILES
-  px4_msgs/rc_channels.msg
-  px4_msgs/vehicle_attitude.msg
-  px4_msgs/rc_channels.msg
+  rc_channels.msg
+  vehicle_attitude.msg
+  vehicle_attitude_setpoint.msg
+  manual_control_setpoint.msg
+  actuator_controls.msg
+  actuator_controls_0.msg
+  vehicle_rates_setpoint.msg
+  vehicle_attitude.msg
+  vehicle_control_mode.msg
+  actuator_armed.msg
+  parameter_update.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -100,11 +110,19 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   src/platforms
   src/include
+  src/modules
+  src/
+  src/lib
+  ${EIGEN_INCLUDE_DIRS}
 )
 
 ## Declare a cpp library
 add_library(px4
   src/platforms/ros/px4_ros_impl.cpp
+  src/platforms/ros/perf_counter.cpp
+  src/platforms/ros/geo.cpp
+  src/lib/mathlib/math/Limits.cpp
+  src/platforms/ros/circuit_breaker.cpp
 )
 
 target_link_libraries(px4
@@ -141,14 +159,16 @@ target_link_libraries(subscriber
   px4
 )
 
-# add_executable(mc_att_control
-	# src/modules/mc_att_control/mc_att_control_main.cpp
-	# src/moudles/mc_att_control/mc_att_control_base.cpp)
-# add_dependencies(mc_att_control px4_generate_messages_cpp)
-# target_link_libraries(mc_att_control
-  # ${catkin_LIBRARIES}
-  # px4
-# )
+## MC Attitude Control
+add_executable(mc_att_control
+	src/modules/mc_att_control/mc_att_control_main.cpp
+	src/modules/mc_att_control/mc_att_control.cpp
+	src/modules/mc_att_control/mc_att_control_base.cpp)
+add_dependencies(mc_att_control px4_generate_messages_cpp)
+target_link_libraries(mc_att_control
+  ${catkin_LIBRARIES}
+  px4
+)
 
 
 #############
diff --git a/Makefile b/Makefile
index f2e467e5aa..910b785a35 100644
--- a/Makefile
+++ b/Makefile
@@ -224,7 +224,7 @@ updatesubmodules:
 	$(Q) (git submodule init)
 	$(Q) (git submodule update)
 
-MSG_DIR = $(PX4_BASE)msg/px4_msgs
+MSG_DIR = $(PX4_BASE)msg
 MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
 TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 38c5ebc7b3..edf4fe1a03 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -82,8 +82,8 @@ MODULES		+= modules/position_estimator_inav
 # Vehicle Control
 #
 #MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
-MODULES		+= modules/fw_pos_control_l1
-MODULES		+= modules/fw_att_control
+#MODULES		+= modules/fw_pos_control_l1
+#MODULES		+= modules/fw_att_control
 MODULES		+= modules/mc_att_control
 MODULES		+= modules/mc_pos_control
 
diff --git a/package.xml b/package.xml
index bc23cdd18f..6662003907 100644
--- a/package.xml
+++ b/package.xml
@@ -43,9 +43,11 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>eigen</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>eigen</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/src/include/px4.h b/src/include/px4.h
index ca702d63ce..34137ee6f4 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -41,6 +41,8 @@
 
 #include "../platforms/px4_includes.h"
 #include "../platforms/px4_defines.h"
+#ifdef __cplusplus
 #include "../platforms/px4_middleware.h"
 #include "../platforms/px4_nodehandle.h"
 #include "../platforms/px4_subscriber.h"
+#endif
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 2311e0a7c9..0127796460 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -44,10 +44,7 @@
  */
 
 #pragma once
-
-#include "uORB/topics/fence.h"
-#include "uORB/topics/vehicle_global_position.h"
-
+#include <px4_defines.h>
 __BEGIN_DECLS
 
 #include "geo_lookup/geo_mag_declination.h"
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 806f5933ad..1e76aae603 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -49,9 +49,8 @@
 #ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
 #else
-#include <math/eigen_math.h>
+#include <platforms/ros/eigen_math.h>
 #include <Eigen/Eigen>
-#define M_PI_2_F 1.5707963267948966192f
 #endif
 
 namespace math
@@ -122,6 +121,15 @@ public:
 		memcpy(data, d, sizeof(data));
 	}
 
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+	/**
+	 * set data from boost::array
+	 */
+	void set(const boost::array<float, 9ul> d) {
+	set(static_cast<const float*>(d.data()));
+	}
+#endif
+
 	/**
 	 * access by index
 	 */
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 57b45e3ab2..20f099831a 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -49,7 +49,7 @@
 #ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
 #else
-#include <math/eigen_math.h>
+#include <platforms/ros/eigen_math.h>
 #endif
 
 namespace math
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index e0bcbc6e9e..53ab743053 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -59,6 +59,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 13bff9561b..906bd0a47a 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -44,6 +44,8 @@
  */
 
 #include "mc_att_control.h"
+#include "mc_att_control_params.h"
+#include "math.h"
 
 #define YAW_DEADZONE	0.05f
 #define MIN_TAKEOFF_THRUST    0.2f
@@ -63,7 +65,6 @@ static const int ERROR = -1;
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	MulticopterAttitudeControlBase(),
 	_task_should_exit(false),
-	_control_task(-1),
 	_actuators_0_circuit_breaker_enabled(false),
 
 	/* publications */
@@ -76,26 +77,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
-	_params_handles.roll_p			= 	PX4_PARAM_INIT("MC_ROLL_P");
-	_params_handles.roll_rate_p		= 	PX4_PARAM_INIT("MC_ROLLRATE_P");
-	_params_handles.roll_rate_i		= 	PX4_PARAM_INIT("MC_ROLLRATE_I");
-	_params_handles.roll_rate_d		= 	PX4_PARAM_INIT("MC_ROLLRATE_D");
-	_params_handles.pitch_p			= 	PX4_PARAM_INIT("MC_PITCH_P");
-	_params_handles.pitch_rate_p		= 	PX4_PARAM_INIT("MC_PITCHRATE_P");
-	_params_handles.pitch_rate_i		= 	PX4_PARAM_INIT("MC_PITCHRATE_I");
-	_params_handles.pitch_rate_d		= 	PX4_PARAM_INIT("MC_PITCHRATE_D");
-	_params_handles.yaw_p			=	PX4_PARAM_INIT("MC_YAW_P");
-	_params_handles.yaw_rate_p		= 	PX4_PARAM_INIT("MC_YAWRATE_P");
-	_params_handles.yaw_rate_i		= 	PX4_PARAM_INIT("MC_YAWRATE_I");
-	_params_handles.yaw_rate_d		= 	PX4_PARAM_INIT("MC_YAWRATE_D");
-	_params_handles.yaw_ff			= 	PX4_PARAM_INIT("MC_YAW_FF");
-	_params_handles.yaw_rate_max		= 	PX4_PARAM_INIT("MC_YAWRATE_MAX");
-	_params_handles.man_roll_max		= 	PX4_PARAM_INIT("MC_MAN_R_MAX");
-	_params_handles.man_pitch_max		= 	PX4_PARAM_INIT("MC_MAN_P_MAX");
-	_params_handles.man_yaw_max		= 	PX4_PARAM_INIT("MC_MAN_Y_MAX");
-	_params_handles.acro_roll_max		= 	PX4_PARAM_INIT("MC_ACRO_R_MAX");
-	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT("MC_ACRO_P_MAX");
-	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT("MC_ACRO_Y_MAX");
+	_params_handles.roll_p			= 	PX4_PARAM_INIT(MC_ROLL_P);
+	_params_handles.roll_rate_p		= 	PX4_PARAM_INIT(MC_ROLLRATE_P);
+	_params_handles.roll_rate_i		= 	PX4_PARAM_INIT(MC_ROLLRATE_I);
+	_params_handles.roll_rate_d		= 	PX4_PARAM_INIT(MC_ROLLRATE_D);
+	_params_handles.pitch_p			= 	PX4_PARAM_INIT(MC_PITCH_P);
+	_params_handles.pitch_rate_p		= 	PX4_PARAM_INIT(MC_PITCHRATE_P);
+	_params_handles.pitch_rate_i		= 	PX4_PARAM_INIT(MC_PITCHRATE_I);
+	_params_handles.pitch_rate_d		= 	PX4_PARAM_INIT(MC_PITCHRATE_D);
+	_params_handles.yaw_p			=	PX4_PARAM_INIT(MC_YAW_P);
+	_params_handles.yaw_rate_p		= 	PX4_PARAM_INIT(MC_YAWRATE_P);
+	_params_handles.yaw_rate_i		= 	PX4_PARAM_INIT(MC_YAWRATE_I);
+	_params_handles.yaw_rate_d		= 	PX4_PARAM_INIT(MC_YAWRATE_D);
+	_params_handles.yaw_ff			= 	PX4_PARAM_INIT(MC_YAW_FF);
+	_params_handles.yaw_rate_max		= 	PX4_PARAM_INIT(MC_YAWRATE_MAX);
+	_params_handles.man_roll_max		= 	PX4_PARAM_INIT(MC_MAN_R_MAX);
+	_params_handles.man_pitch_max		= 	PX4_PARAM_INIT(MC_MAN_P_MAX);
+	_params_handles.man_yaw_max		= 	PX4_PARAM_INIT(MC_MAN_Y_MAX);
+	_params_handles.acro_roll_max		= 	PX4_PARAM_INIT(MC_ACRO_R_MAX);
+	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT(MC_ACRO_P_MAX);
+	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT(MC_ACRO_Y_MAX);
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -115,26 +116,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()
 {
-	if (_control_task != -1) {
-		/* task wakes up every 100ms or so at the longest */
-		_task_should_exit = true;
-
-		/* wait for a second for the task to quit at our request */
-		unsigned i = 0;
-
-		do {
-			/* wait 20ms */
-			usleep(20000);
-
-			/* if we have given up, kill it */
-			if (++i > 50) {
-				task_delete(_control_task);
-				break;
-			}
-		} while (_control_task != -1);
-	}
-
-	// mc_att_control::g_control = nullptr;
 }
 
 int
@@ -203,8 +184,8 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 	/* run controller on attitude changes */
 	static uint64_t last_run = 0;
-	float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
+	float dt = (px4::get_time_micros() - last_run) / 1000000.0f;
+	last_run = px4::get_time_micros();
 
 	/* guard against too small (< 2ms) and too large (> 20ms) dt's */
 	if (dt < 0.002f) {
@@ -219,7 +200,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 		/* publish the attitude setpoint if needed */
 		if (_publish_att_sp) {
-			_v_att_sp_mod.timestamp = hrt_absolute_time();
+			_v_att_sp_mod.timestamp = px4::get_time_micros();
 
 			if (_att_sp_pub != nullptr) {
 				_att_sp_pub->publish(_v_att_sp_mod);
@@ -234,7 +215,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		_v_rates_sp_mod.pitch = _rates_sp(1);
 		_v_rates_sp_mod.yaw = _rates_sp(2);
 		_v_rates_sp_mod.thrust = _thrust_sp;
-		_v_rates_sp_mod.timestamp = hrt_absolute_time();
+		_v_rates_sp_mod.timestamp = px4::get_time_micros();
 
 		if (_v_rates_sp_pub != nullptr) {
 			_v_rates_sp_pub->publish(_v_rates_sp_mod);
@@ -258,7 +239,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 			_v_rates_sp_mod.pitch = _rates_sp(1);
 			_v_rates_sp_mod.yaw = _rates_sp(2);
 			_v_rates_sp_mod.thrust = _thrust_sp;
-			_v_rates_sp_mod.timestamp = hrt_absolute_time();
+			_v_rates_sp_mod.timestamp = px4::get_time_micros();
 
 			if (_v_rates_sp_pub != nullptr) {
 				_v_rates_sp_pub->publish(_v_rates_sp_mod);
@@ -285,7 +266,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
 		_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 		_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
-		_actuators.timestamp = hrt_absolute_time();
+		_actuators.timestamp = px4::get_time_micros();
 
 		if (!_actuators_0_circuit_breaker_enabled) {
 			if (_actuators_0_pub != nullptr) {
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 24009a5e66..1da7384085 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -52,22 +52,16 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
-#include <px4.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
+// #include <systemlib/systemlib.h>
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
-#include <lib/geo/geo.h>
 
 #include "mc_att_control_base.h"
 
@@ -91,7 +85,6 @@ public:
 
 private:
 	bool		_task_should_exit;			/**< if true, sensor task should exit */
-	int		_control_task;				/**< task handle for sensor task */
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
 	px4::Publisher *	_att_sp_pub;			/**< attitude setpoint publication */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a1dca8a8c9..be627866e4 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -129,12 +129,12 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
 
 PX4_MAIN_FUNCTION(mc_att_control)
 {
-	warnx("starting");
+	PX4_INFO("starting");
 	MulticopterAttitudeControl attctl;
 	thread_running = true;
 	attctl.spin();
 
-	warnx("exiting.");
+	PX4_INFO("exiting.");
 	thread_running = false;
 	return 0;
 }
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
index 12187d70ea..1b3ffd59fe 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -42,7 +42,8 @@
  * parameter needs to set to the key (magic).
  */
 
-#include <systemlib/param/param.h>
+#include <px4.h>
+#include <systemlib/circuit_breaker_params.h>
 #include <systemlib/circuit_breaker.h>
 
 /**
@@ -56,7 +57,7 @@
  * @max 894281
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
 
 /**
  * Circuit breaker for rate controller output
@@ -69,7 +70,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
  * @max 140253
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
 
 /**
  * Circuit breaker for IO safety
@@ -81,7 +82,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
  * @max 22027
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
 
 /**
  * Circuit breaker for airspeed sensor
@@ -93,7 +94,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
  * @max 162128
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
 
 /**
  * Circuit breaker for flight termination
@@ -106,7 +107,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
  * @max 121212
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
+PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
 
 /**
  * Circuit breaker for engine failure detection
@@ -120,7 +121,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
  * @max 284953
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
+PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
 
 /**
  * Circuit breaker for gps failure detection
@@ -134,7 +135,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
  * @max 240024
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
+PX4_PARAM_DEFINE_INT32(CBRK_GPSFAIL);
 
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index b3431722fc..012d8cb61e 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -61,8 +61,14 @@
 
 __BEGIN_DECLS
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
+#ifdef __cplusplus
+}
+#endif
 __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
-
 __END_DECLS
 
 #endif /* CIRCUIT_BREAKER_H_ */
diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h
new file mode 100644
index 0000000000..768bf7f533
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker_params.h
@@ -0,0 +1,7 @@
+#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
+#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
+#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
+#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
+#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
+#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
+#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 668d9dfdf6..a9dfb13f8b 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -40,6 +40,7 @@
 #define _SYSTEMLIB_PERF_COUNTER_H value
 
 #include <stdint.h>
+#include <platforms/px4_defines.h>
 
 /**
  * Counter types.
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index a1bf9919e9..712e0dd63d 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -51,7 +51,10 @@
  * Building for running within the ROS environment
  */
 #define __EXPORT
+#define noreturn_function
+#ifdef __cplusplus
 #include "ros/ros.h"
+#endif
 /* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
 
@@ -63,7 +66,7 @@
 #define PX4_TOPIC(_name) #_name
 
 /* Topic type */
-#define PX4_TOPIC_T(_name) _name
+#define PX4_TOPIC_T(_name) px4::_name
 
 /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr);
@@ -93,6 +96,38 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 
+#define OK 0
+#define ERROR -1
+
+//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work
+#define isfinite(_value) std::isfinite(_value)
+
+/* Useful constants.  */
+#define M_E_F			2.7182818284590452354f
+#define M_LOG2E_F		1.4426950408889634074f
+#define M_LOG10E_F		0.43429448190325182765f
+#define M_LN2_F			_M_LN2_F
+#define M_LN10_F		2.30258509299404568402f
+#define M_PI_F			3.14159265358979323846f
+#define M_TWOPI_F       (M_PI_F * 2.0f)
+#define M_PI_2_F		1.57079632679489661923f
+#define M_PI_4_F		0.78539816339744830962f
+#define M_3PI_4_F		2.3561944901923448370E0f
+#define M_SQRTPI_F      1.77245385090551602792981f
+#define M_1_PI_F		0.31830988618379067154f
+#define M_2_PI_F		0.63661977236758134308f
+#define M_2_SQRTPI_F	1.12837916709551257390f
+#define M_DEG_TO_RAD_F 	0.01745329251994f
+#define M_RAD_TO_DEG_F 	57.2957795130823f
+#define M_SQRT2_F		1.41421356237309504880f
+#define M_SQRT1_2_F		0.70710678118654752440f
+#define M_LN2LO_F       1.9082149292705877000E-10f
+#define M_LN2HI_F       6.9314718036912381649E-1f
+#define M_SQRT3_F		1.73205080756887719000f
+#define M_IVLN10_F      0.43429448190325182765f /* 1 / log(10) */
+#define M_LOG2_E_F      _M_LN2_F
+#define M_INVLN2_F      1.4426950408889633870E0f  /* 1 / log(2) */
+
 #else
 /*
  * Building for NuttX
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 525f94aae9..752c6b5fe9 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -45,8 +45,21 @@
 /*
  * Building for running within the ROS environment
  */
+
+#ifdef __cplusplus
 #include "ros/ros.h"
 #include "px4/rc_channels.h"
+#include "px4/vehicle_attitude.h"
+#include <px4/vehicle_attitude_setpoint.h>
+#include <px4/manual_control_setpoint.h>
+#include <px4/actuator_controls.h>
+#include <px4/actuator_controls_0.h>
+#include <px4/vehicle_rates_setpoint.h>
+#include <px4/vehicle_attitude.h>
+#include <px4/vehicle_control_mode.h>
+#include <px4/actuator_armed.h>
+#include <px4/parameter_update.h>
+#endif
 
 #else
 /*
diff --git a/src/platforms/ros/circuit_breaker.cpp b/src/platforms/ros/circuit_breaker.cpp
new file mode 100644
index 0000000000..1e912d3ac7
--- /dev/null
+++ b/src/platforms/ros/circuit_breaker.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file circuit_breaker.c
+ *
+ * Circuit breaker parameters.
+ * Analog to real aviation circuit breakers these parameters
+ * allow to disable subsystems. They are not supported as standard
+ * operation procedure and are only provided for development purposes.
+ * To ensure they are not activated accidentally, the associated
+ * parameter needs to set to the key (magic).
+ */
+
+#include <px4.h>
+#include <systemlib/circuit_breaker_params.h>
+#include <systemlib/circuit_breaker.h>
+
+bool circuit_breaker_enabled(const char* breaker, int32_t magic)
+{
+	int32_t val;
+	(void)PX4_PARAM_GET(breaker, &val);
+
+	return (val == magic);
+}
+
diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
new file mode 100644
index 0000000000..a7ee6fd844
--- /dev/null
+++ b/src/platforms/ros/geo.cpp
@@ -0,0 +1,165 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file geo.c
+ *
+ * Geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <geo/geo.h>
+#include <px4.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+#include <string.h>
+#include <float.h>
+
+__EXPORT float _wrap_pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+	while (bearing >= M_PI_F) {
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+	while (bearing < -M_PI_F) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_2pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+	while (bearing >= M_TWOPI_F) {
+		bearing -= M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+	while (bearing < 0.0f) {
+		bearing += M_TWOPI_F;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_180(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+	while (bearing >= 180.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+	while (bearing < -180.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_360(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	int c = 0;
+	while (bearing >= 360.0f) {
+		bearing -= 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	c = 0;
+	while (bearing < 0.0f) {
+		bearing += 360.0f;
+
+		if (c++ > 3) {
+			return NAN;
+		}
+	}
+
+	return bearing;
+}

From e1ff89ad6131019a12114ca7b0b5fa40112b9645 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 16 Dec 2014 10:11:59 +0100
Subject: [PATCH 176/416] add nuttx platform to fmuv1 makefile

---
 makefiles/config_px4fmu-v1_default.mk | 1 +
 1 file changed, 1 insertion(+)

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 18be47065d..7eb090d732 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -110,6 +110,7 @@ MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
 
 #
 # Demo apps

From 71f6a34367794a887704e2898f8a10101bacfb12 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 16 Dec 2014 10:12:50 +0100
Subject: [PATCH 177/416] mc att: increase stack size

---
 src/modules/mc_att_control/mc_att_control_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index be627866e4..4188c7880b 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -99,7 +99,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       3000,
 				       mc_att_control_task_main,
 					(argv) ? (const char **)&argv[2] : (const char **)NULL);
 

From be269520382adbd4bea59c439599897a53109ad7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 08:03:22 +0100
Subject: [PATCH 178/416] px4 nodehandle: nuttx: call spin once also after
 timeout

---
 src/platforms/px4_nodehandle.h | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 9bd792c697..8879148a39 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -228,12 +228,7 @@ public:
 			struct pollfd pfd;
 			pfd.fd = _sub_min_interval->getHandle();
 			pfd.events = POLLIN;
-
-			if (poll(&pfd, 1, timeout_ms) <= 0) {
-				/* timed out */
-				continue;
-			}
-
+			poll(&pfd, 1, timeout_ms);
 			spinOnce();
 		}
 	}

From 9bad23e41852eba6657898d4ea46b0b303de4ae3 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 08:22:38 +0100
Subject: [PATCH 179/416] add explicit non-callback contructor for nuttx/uorb
 subscriber to work around linker issues

---
 src/platforms/px4_defines.h    |  2 +-
 src/platforms/px4_nodehandle.h | 20 ++++++++++++++++++++
 src/platforms/px4_subscriber.h | 15 +++++++++++++++
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 712e0dd63d..c76548381b 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -154,7 +154,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
 /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), nullptr, _interval)
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), _interval)
 
 /* Parameter handle datatype */
 #include <systemlib/param/param.h>
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 8879148a39..1608868101 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -178,6 +178,26 @@ public:
 		return (Subscriber<M> *)sub_px4;
 	}
 
+	/**
+	 * Subscribe without callback to function
+	 * @param meta		Describes the topic which nodehande should subscribe to
+	 * @param interval	Minimal interval between data fetches from orb
+	 */
+
+	template<typename M>
+	Subscriber<M> *subscribe(const struct orb_metadata *meta,
+			      unsigned interval)
+	{
+		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
+
+		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
+			_sub_min_interval = sub_px4;
+		}
+
+		return (Subscriber<M> *)sub_px4;
+	}
+
 	/**
 	 * Advertise topic
 	 * @param meta		Describes the topic which is advertised
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 7d8463cb52..5d4e67ad01 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -178,6 +178,21 @@ public:
 		//XXX store callback
 	{}
 
+	/**
+	 * Construct SubscriberUORB by providing orb meta data without callback
+	 * @param meta	    orb metadata for the topic which is used
+	 * @param interval	Minimal interval between calls to callback
+	 * @param list	    subscriber is added to this list
+	 */
+	SubscriberUORB(const struct orb_metadata *meta,
+		      unsigned interval,
+		      List<uORB::SubscriptionNode *> *list) :
+		Subscriber<M>(),
+		uORB::Subscription<M>(meta, interval, list),
+		_callback(nullptr)
+		//XXX store callback
+	{}
+
 	~SubscriberUORB() {};
 
 	/**

From bbfe78e4f60de5414cf3594250c5ed769f0a1433 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 13:50:29 +0100
Subject: [PATCH 180/416] mc att ctl: fix subscription handlers, fix parameters

---
 src/modules/mc_att_control/mc_att_control.cpp | 20 +++++++++++--------
 src/modules/mc_att_control/mc_att_control.h   |  4 +++-
 .../mc_att_control/mc_att_control_base.h      |  4 ++--
 .../mc_att_control/mc_att_control_params.c    |  3 ++-
 4 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 906bd0a47a..89da8438b2 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -104,13 +104,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	/*
 	 * do subscriptions
 	 */
-	PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
-	PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
-	PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
-	PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
-	PX4_SUBSCRIBE(_n, parameter_update, 0);
-	PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
-	PX4_SUBSCRIBE(_n, actuator_armed, 0);
+	_v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
+	_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
+	_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
+	PX4_SUBSCRIBE(_n, parameter_update, MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+	_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
+	_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
 
 }
 
@@ -178,6 +178,11 @@ MulticopterAttitudeControl::parameters_update()
 	return OK;
 }
 
+void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg)
+{
+	parameters_update();
+}
+
 void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
 
 	perf_begin(_loop_perf);
@@ -250,7 +255,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 		} else {
 			/* attitude controller disabled, poll rates setpoint topic */
-			//XXX vehicle_rates_setpoint_poll();
 			_rates_sp(0) = _v_rates_sp->get().roll;
 			_rates_sp(1) = _v_rates_sp->get().pitch;
 			_rates_sp(2) = _v_rates_sp->get().yaw;
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 1da7384085..76c095cc1a 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -79,7 +79,9 @@ public:
 	 */
 	~MulticopterAttitudeControl();
 
-	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+	void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg);
 
 	void spin() { _n.spin(); }
 
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 2135b61d82..17c7797967 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -90,8 +90,8 @@ protected:
 	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;  /**< manual control setpoint */
 	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
 	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
-	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) * _v_att_sp;	/**< vehicle attitude setpoint */
-	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) * _v_rates_sp;	/**< vehicle rates setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	/**< vehicle attitude setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	/**< vehicle rates setpoint */
 
 	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
 								  that gets published eventually */
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index bbbfd829bb..a0b1706f2d 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -40,7 +40,8 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include "mc_att_control_params.h""
+#include <px4_defines.h>
+#include "mc_att_control_params.h"
 #include <systemlib/param/param.h>
 
 /**

From 2d0f11783c1394b44d31d6bd6d07612c164dbe90 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 15:47:18 +0100
Subject: [PATCH 181/416] fix header for cpp perf_counter dummy

---
 src/platforms/ros/perf_counter.cpp | 44 ++++++++++++++++++++++++++----
 1 file changed, 39 insertions(+), 5 deletions(-)

diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp
index aa8d85c60c..a718013975 100755
--- a/src/platforms/ros/perf_counter.cpp
+++ b/src/platforms/ros/perf_counter.cpp
@@ -1,9 +1,43 @@
-/*
- * perf_counter.c
-
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file perf_counter.cpp
+ *
+ * Empty function calls for ros compatibility
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
- *  Created on: Sep 24, 2014
- *      Author: roman
  */
 #include <stdlib.h>
 #include <stdio.h>

From 7ad3b0335336d65736f707486b2b714d0fa91c2a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 15:49:03 +0100
Subject: [PATCH 182/416] multi platform interface: macro to get param by name

---
 src/platforms/px4_defines.h | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index c76548381b..1881490088 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -93,9 +93,12 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 /* Initialize a param, in case of ROS the parameter is sent to the parameter server here */
 #define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 
-/* Get value of parameter */
+/* Get value of parameter by handle */
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 
+/* Get value of parameter by name, which is equal to the handle for ros */
+#define PX4_PARAM_GET_NAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt)
+
 #define OK 0
 #define ERROR -1
 
@@ -163,9 +166,12 @@ typedef param_t px4_param_t;
 /* Initialize a param, get param handle */
 #define PX4_PARAM_INIT(_name) param_find(#_name)
 
-/* Get value of parameter */
+/* Get value of parameter by handle */
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 
+/* Get value of parameter by name */
+#define PX4_PARAM_GET_NAME(_name, _destpt) param_get(PX4_PARAM_INIT(_name), _destpt)
+
 /* XXX this is a hack to resolve conflicts with NuttX headers */
 #define isspace(c) \
   ((c) == ' '  || (c) == '\t' || (c) == '\n' || \

From 0a1e94d504b38d30c2d7428300c5728798db094d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 15:50:01 +0100
Subject: [PATCH 183/416] circuit breaker: move to cpp, all platforms use the
 same file

---
 CMakeLists.txt                                |  2 +-
 .../systemlib}/circuit_breaker.cpp            |  4 ++--
 src/modules/systemlib/circuit_breaker.h       | 10 ++------
 ...uit_breaker.c => circuit_breaker_params.c} | 24 -------------------
 src/modules/systemlib/module.mk               |  3 ++-
 5 files changed, 7 insertions(+), 36 deletions(-)
 rename src/{platforms/ros => modules/systemlib}/circuit_breaker.cpp (96%)
 rename src/modules/systemlib/{circuit_breaker.c => circuit_breaker_params.c} (87%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 19a14f62af..3184bb2ac8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -122,7 +122,7 @@ add_library(px4
   src/platforms/ros/perf_counter.cpp
   src/platforms/ros/geo.cpp
   src/lib/mathlib/math/Limits.cpp
-  src/platforms/ros/circuit_breaker.cpp
+  src/modules/systemlib/circuit_breaker.cpp
 )
 
 target_link_libraries(px4
diff --git a/src/platforms/ros/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp
similarity index 96%
rename from src/platforms/ros/circuit_breaker.cpp
rename to src/modules/systemlib/circuit_breaker.cpp
index 1e912d3ac7..2df13ebff6 100644
--- a/src/platforms/ros/circuit_breaker.cpp
+++ b/src/modules/systemlib/circuit_breaker.cpp
@@ -43,13 +43,13 @@
  */
 
 #include <px4.h>
-#include <systemlib/circuit_breaker_params.h>
 #include <systemlib/circuit_breaker.h>
 
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
 	int32_t val;
-	(void)PX4_PARAM_GET(breaker, &val);
+	/* (void)param_get(param_find(breaker), &val); */
+	(void)PX4_PARAM_GET_NAME(breaker, &val);
 
 	return (val == magic);
 }
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index 012d8cb61e..c97dbc26ff 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -61,14 +61,8 @@
 
 __BEGIN_DECLS
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
-#ifdef __cplusplus
-}
-#endif
-__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
+extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
+
 __END_DECLS
 
 #endif /* CIRCUIT_BREAKER_H_ */
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker_params.c
similarity index 87%
rename from src/modules/systemlib/circuit_breaker.c
rename to src/modules/systemlib/circuit_breaker_params.c
index 1b3ffd59fe..e499ae27ac 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -44,7 +44,6 @@
 
 #include <px4.h>
 #include <systemlib/circuit_breaker_params.h>
-#include <systemlib/circuit_breaker.h>
 
 /**
  * Circuit breaker for power supply check
@@ -122,26 +121,3 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
  * @group Circuit Breaker
  */
 PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
-
-/**
- * Circuit breaker for gps failure detection
- *
- * Setting this parameter to 240024 will disable the gps failure detection.
- * If the aircraft is in gps failure mode the gps failure flag will be
- * set to healthy
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @min 0
- * @max 240024
- * @group Circuit Breaker
- */
-PX4_PARAM_DEFINE_INT32(CBRK_GPSFAIL);
-
-bool circuit_breaker_enabled(const char* breaker, int32_t magic)
-{
-	int32_t val;
-	(void)param_get(param_find(breaker), &val);
-
-	return (val == magic);
-}
-
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index fe8b7e75a0..1e0a9c0074 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,7 +53,8 @@ SRCS		 = err.c \
 		   otp.c \
 		   board_serial.c \
 		   pwm_limit/pwm_limit.c \
-		   circuit_breaker.c \
+		   circuit_breaker.cpp \
+		   circuit_breaker_params.c \
 		   mcu_version.c
 
 MAXOPTIMIZATION	 = -Os

From 4402d7106bc63b2a02a1e4c22f54e072b3c48fc7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 17 Dec 2014 16:31:40 +0100
Subject: [PATCH 184/416] improve and fix multiplatform param by name macro

---
 src/modules/systemlib/circuit_breaker.cpp | 3 +--
 src/platforms/px4_defines.h               | 4 ++--
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp
index 2df13ebff6..ea478a60fa 100644
--- a/src/modules/systemlib/circuit_breaker.cpp
+++ b/src/modules/systemlib/circuit_breaker.cpp
@@ -48,8 +48,7 @@
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
 	int32_t val;
-	/* (void)param_get(param_find(breaker), &val); */
-	(void)PX4_PARAM_GET_NAME(breaker, &val);
+	(void)PX4_PARAM_GET_BYNAME(breaker, &val);
 
 	return (val == magic);
 }
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 1881490088..c1a0782c49 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -97,7 +97,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 
 /* Get value of parameter by name, which is equal to the handle for ros */
-#define PX4_PARAM_GET_NAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt)
+#define PX4_PARAM_GET_BYNAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt)
 
 #define OK 0
 #define ERROR -1
@@ -170,7 +170,7 @@ typedef param_t px4_param_t;
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 
 /* Get value of parameter by name */
-#define PX4_PARAM_GET_NAME(_name, _destpt) param_get(PX4_PARAM_INIT(_name), _destpt)
+#define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt)
 
 /* XXX this is a hack to resolve conflicts with NuttX headers */
 #define isspace(c) \

From e697413667579ffb1c5900193a03da257c05d11a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 18 Dec 2014 12:11:35 +0100
Subject: [PATCH 185/416] multiplatform subscription: uorb: separate class for
 no callback case

---
 src/platforms/px4_nodehandle.h |  2 +-
 src/platforms/px4_subscriber.h | 73 +++++++++++++++++++++-------------
 2 files changed, 47 insertions(+), 28 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 1608868101..97fb25563c 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -168,7 +168,7 @@ public:
 			      std::function<void(const M &)> callback,
 			      unsigned interval)
 	{
-		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, callback, &_subs);
+		SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
 		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 5d4e67ad01..8c299c7489 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -161,22 +161,6 @@ class SubscriberUORB :
 	public uORB::Subscription<M>
 {
 public:
-	/**
-	 * Construct SubscriberUORB by providing orb meta data
-	 * @param meta	    orb metadata for the topic which is used
-	 * @param callback	Callback, executed on receiving a new message
-	 * @param interval	Minimal interval between calls to callback
-	 * @param list	    subscriber is added to this list
-	 */
-	SubscriberUORB(const struct orb_metadata *meta,
-		      unsigned interval,
-		      std::function<void(const M &)> callback,
-		      List<uORB::SubscriptionNode *> *list) :
-		Subscriber<M>(),
-		uORB::Subscription<M>(meta, interval, list),
-		_callback(callback)
-		//XXX store callback
-	{}
 
 	/**
 	 * Construct SubscriberUORB by providing orb meta data without callback
@@ -188,19 +172,61 @@ public:
 		      unsigned interval,
 		      List<uORB::SubscriptionNode *> *list) :
 		Subscriber<M>(),
-		uORB::Subscription<M>(meta, interval, list),
-		_callback(nullptr)
-		//XXX store callback
+		uORB::Subscription<M>(meta, interval, list)
 	{}
 
 	~SubscriberUORB() {};
 
+	/**
+	 * Update Subscription
+	 * Invoked by the list traversal in NodeHandle::spinOnce
+	 */
+	virtual void update()
+	{
+		if (!uORB::Subscription<M>::updated()) {
+			/* Topic not updated */
+			return;
+		}
+
+		/* get latest data */
+		uORB::Subscription<M>::update();
+	};
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	const M& get() { return uORB::Subscription<M>::getData(); }
+};
+
+template<typename M>
+class SubscriberUORBCallback :
+	public SubscriberUORB<M>
+{
+public:
+	/**
+	 * Construct SubscriberUORBCallback by providing orb meta data
+	 * @param meta	    orb metadata for the topic which is used
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 * @param list	    subscriber is added to this list
+	 */
+	SubscriberUORBCallback(const struct orb_metadata *meta,
+		      unsigned interval,
+		      std::function<void(const M &)> callback,
+		      List<uORB::SubscriptionNode *> *list) :
+		SubscriberUORB<M>(meta, interval, list),
+		_callback(callback)
+	{}
+
+	~SubscriberUORBCallback() {};
+
 	/**
 	 * Update Subscription
 	 * Invoked by the list traversal in NodeHandle::spinOnce
 	 * If new data is available the callback is called
 	 */
-	void update()
+	virtual void update()
 	{
 		if (!uORB::Subscription<M>::updated()) {
 			/* Topic not updated, do not call callback */
@@ -221,16 +247,9 @@ public:
 
 	};
 
-	/* Accessors*/
-	/**
-	 * Get the last message value
-	 */
-	const M& get() { return uORB::Subscription<M>::getData(); }
-
 protected:
 	std::function<void(const M &)> _callback;	/**< Callback handle,
 							  called when new data is available */
-
 };
 #endif
 

From ad189cf7d69b8de16244b90d398e1d84ed6d0f4b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 19 Dec 2014 16:02:13 +0100
Subject: [PATCH 186/416] fix small compile error after merge

---
 .../fw_att_control/fw_att_control_main.cpp    | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 53e854ae54..0025aae7fe 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -744,15 +744,15 @@ FixedwingAttitudeControl::task_main()
 				_att.roll    = euler_angles(0);
 				_att.pitch   = euler_angles(1);
 				_att.yaw     = euler_angles(2);
-				_att.R[0][0] = R_adapted(0, 0);
-				_att.R[0][1] = R_adapted(0, 1);
-				_att.R[0][2] = R_adapted(0, 2);
-				_att.R[1][0] = R_adapted(1, 0);
-				_att.R[1][1] = R_adapted(1, 1);
-				_att.R[1][2] = R_adapted(1, 2);
-				_att.R[2][0] = R_adapted(2, 0);
-				_att.R[2][1] = R_adapted(2, 1);
-				_att.R[2][2] = R_adapted(2, 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);
 
 				// lastly, roll- and yawspeed have to be swaped
 				float helper = _att.rollspeed;
@@ -850,7 +850,7 @@ FixedwingAttitudeControl::task_main()
 						_yaw_ctrl.reset_integrator();
 					}
 				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
- 					/* 
+ 					/*
 					 * Velocity should be controlled and manual is enabled.
 					*/
 					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)

From e31e143047712aa2f43a4a900a0a1210082a36cf Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 10:00:09 +0100
Subject: [PATCH 187/416] fix include

---
 src/platforms/px4_includes.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 6c36b692a8..9c8c24764f 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -59,7 +59,7 @@
 #include <px4/vehicle_control_mode.h>
 #include <px4/actuator_armed.h>
 #include <px4/parameter_update.h>
-#include <px4/parameter_status.h>
+#include <px4/vehicle_status.h>
 #endif
 
 #else

From 4567512d7a341659bcb19956d58201aeaf0871b5 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 10:01:20 +0100
Subject: [PATCH 188/416] add actuator_controls_virtual_mc as msg, fix msg
 setup in CMakeList

---
 CMakeLists.txt                       | 2 ++
 msg/actuator_controls_virtual_mc.msg | 4 ++++
 2 files changed, 6 insertions(+)
 create mode 100644 msg/actuator_controls_virtual_mc.msg

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3184bb2ac8..dfcbd4adc3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,11 +56,13 @@ add_message_files(
   manual_control_setpoint.msg
   actuator_controls.msg
   actuator_controls_0.msg
+  actuator_controls_virtual_mc.msg
   vehicle_rates_setpoint.msg
   vehicle_attitude.msg
   vehicle_control_mode.msg
   actuator_armed.msg
   parameter_update.msg
+  vehicle_status.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/actuator_controls_virtual_mc.msg
new file mode 100644
index 0000000000..743f20cdff
--- /dev/null
+++ b/msg/actuator_controls_virtual_mc.msg
@@ -0,0 +1,4 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+float32[8] control

From 382158d87b170e2b1848928c745073c209ac64f3 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 10:03:46 +0100
Subject: [PATCH 189/416] fix merge error in .gitmodules

---
 .gitmodules | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/.gitmodules b/.gitmodules
index 577181281b..d0039b38d8 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -13,6 +13,3 @@
 [submodule "Tools/gencpp"]
 	path = Tools/gencpp
 	url = https://github.com/ros/gencpp.git
-[submodule "src/lib/uavcan"]
-	path = src/lib/uavcan
-	url = git://github.com/pavel-kirienko/uavcan.git

From ef61ba5454153c8cff21345f4930203cc22505c7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 10:43:19 +0100
Subject: [PATCH 190/416] fix compile/merge error in mc_att_control

---
 src/modules/mc_att_control/mc_att_control.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 5dfe18cf2c..66ceb6d778 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -224,7 +224,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		if (_v_rates_sp_pub != nullptr) {
 			_v_rates_sp_pub->publish(_v_rates_sp_mod);
 		} else {
-			if (_v_status->get()._is_vtol) {
+			if (_v_status->get().is_vtol) {
 				_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
 			} else {
 				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
@@ -253,7 +253,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 				_v_rates_sp_pub->publish(_v_rates_sp_mod);
 
 			} else {
-				if (_v_status->get()._is_vtol) {
+				if (_v_status->get().is_vtol) {
 					_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
 				} else {
 					_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
@@ -284,7 +284,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 				_actuators_0_pub->publish(_actuators);
 
 			} else {
-				if (_v_status()->get()._is_vtol) {
+				if (_v_status->get().is_vtol) {
 					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc);
 				} else {
 					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);

From a93362c70013281457fb44ab0494157325eb28f9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 11:50:41 +0100
Subject: [PATCH 191/416] re-add accidentally removed parameter init

---
 src/modules/mc_att_control/mc_att_control.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 66ceb6d778..822a504b52 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -98,6 +98,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT(MC_ACRO_P_MAX);
 	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT(MC_ACRO_Y_MAX);
 
+	/* fetch initial parameter values */
+	parameters_update();
+
 	/*
 	 * do subscriptions
 	 */

From 195472fddf0242bfb881aef9e53d910c81a2b003 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 12:51:38 +0100
Subject: [PATCH 192/416] fix init of published structs

---
 src/modules/mc_att_control/mc_att_control_base.cpp |  8 ++++----
 src/platforms/px4_subscriber.h                     | 12 ++++++++++++
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index d7fbf3acd3..2315e0a993 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -58,8 +58,8 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_publish_att_sp(false)
 
 {
-	memset(&_v_rates_sp, 0, sizeof(_v_att_sp_mod));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod));
+	memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod));
+	memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod));
 	memset(&_actuators, 0, sizeof(_actuators));
 
 	_params.att_p.zero();
@@ -98,7 +98,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		if (_v_control_mode->get().flag_control_velocity_enabled
 		    || _v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			_v_att_sp_mod = _v_att_sp->get();
+			memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod));
 		}
 
 		if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
@@ -157,7 +157,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		_v_att_sp_mod = _v_att_sp->get();
+		memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod));
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 8c299c7489..e7e344201f 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -80,6 +80,10 @@ public:
 	 * Get the last message value
 	 */
 	virtual const M& get() = 0;
+	/**
+	 * Get void pointer to last message value
+	 */
+	virtual void * get_void_ptr() = 0;
 };
 
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
@@ -121,6 +125,10 @@ public:
 	 * Get the last message value
 	 */
 	const M& get() { return _msg_current; }
+	/**
+	 * Get void pointer to last message value
+	 */
+	void * get_void_ptr() { return (void*)&_msg_current; }
 
 protected:
 	/**
@@ -197,6 +205,10 @@ public:
 	 * Get the last message value
 	 */
 	const M& get() { return uORB::Subscription<M>::getData(); }
+	/**
+	 * Get void pointer to last message value
+	 */
+	void * get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); }
 };
 
 template<typename M>

From acb4844939f971a1a33515316f5e6c7c8f668f12 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 25 Dec 2014 18:21:24 +0100
Subject: [PATCH 193/416] don't publish att sp in altctl because it can mask
 the setpoint from the pos controller

---
 src/modules/mc_att_control/mc_att_control_base.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 2315e0a993..aff59778ee 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -135,7 +135,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			}
 
 			_v_att_sp_mod.R_valid = false;
-			_publish_att_sp = true;
+			// _publish_att_sp = true;
 		}
 
 		/* reset yaw setpint to current position if needed */
@@ -143,7 +143,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			_reset_yaw_sp = false;
 			_v_att_sp_mod.yaw_body = _v_att->get().yaw;
 			_v_att_sp_mod.R_valid = false;
-			_publish_att_sp = true;
+			// _publish_att_sp = true;
 		}
 
 		if (!_v_control_mode->get().flag_control_velocity_enabled) {
@@ -152,7 +152,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			_v_att_sp_mod.pitch_body = -_manual_control_sp->get().x
 					       * _params.man_pitch_max;
 			_v_att_sp_mod.R_valid = false;
-			_publish_att_sp = true;
+			// _publish_att_sp = true;
 		}
 
 	} else {

From ac8b47b0c32d41cb4cf0f03d996bc8a4c77a0f49 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 29 Dec 2014 09:49:53 +0100
Subject: [PATCH 194/416] add missing msg and includes

---
 CMakeLists.txt                    | 1 +
 msg/mc_virtual_rates_setpoint.msg | 6 ++++++
 src/platforms/px4_includes.h      | 2 ++
 3 files changed, 9 insertions(+)
 create mode 100644 msg/mc_virtual_rates_setpoint.msg

diff --git a/CMakeLists.txt b/CMakeLists.txt
index dfcbd4adc3..519afa0e29 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,6 +58,7 @@ add_message_files(
   actuator_controls_0.msg
   actuator_controls_virtual_mc.msg
   vehicle_rates_setpoint.msg
+  mc_virtual_rates_setpoint.msg
   vehicle_attitude.msg
   vehicle_control_mode.msg
   actuator_armed.msg
diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg
new file mode 100644
index 0000000000..834113c798
--- /dev/null
+++ b/msg/mc_virtual_rates_setpoint.msg
@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 9c8c24764f..4d29bc3cd8 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -54,7 +54,9 @@
 #include <px4/manual_control_setpoint.h>
 #include <px4/actuator_controls.h>
 #include <px4/actuator_controls_0.h>
+#include <px4/actuator_controls_virtual_mc.h>
 #include <px4/vehicle_rates_setpoint.h>
+#include <px4/mc_virtual_rates_setpoint.h>
 #include <px4/vehicle_attitude.h>
 #include <px4/vehicle_control_mode.h>
 #include <px4/actuator_armed.h>

From 9d30c297f40b06779db12e44f2068b482a7fae64 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 29 Dec 2014 10:17:25 +0100
Subject: [PATCH 195/416] add ros launch files

---
 launch/example.launch     | 8 ++++++++
 launch/multicopter.launch | 7 +++++++
 2 files changed, 15 insertions(+)
 create mode 100644 launch/example.launch
 create mode 100644 launch/multicopter.launch

diff --git a/launch/example.launch b/launch/example.launch
new file mode 100644
index 0000000000..39da4b3d42
--- /dev/null
+++ b/launch/example.launch
@@ -0,0 +1,8 @@
+<launch>
+
+<group ns="px4_example">
+	<node pkg="px4" name="subscriber" type="subscriber"/>
+	<node pkg="px4" name="publisher" type="publisher"/>
+</group>
+
+</launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
new file mode 100644
index 0000000000..57b7653d50
--- /dev/null
+++ b/launch/multicopter.launch
@@ -0,0 +1,7 @@
+<launch>
+
+<group ns="px4_example">
+	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
+</group>
+
+</launch>

From b04c80aec7ee91820e5ffa5a09e40aa30b285bc7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 29 Dec 2014 10:35:46 +0100
Subject: [PATCH 196/416] mc att: fix init call

---
 src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 4188c7880b..080f0ed65c 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -80,8 +80,6 @@ void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
 
 extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
 {
-	px4::init(argc, argv, "mc_att_control");
-
 	if (argc < 1) {
 		errx(1, "usage: mc_att_control {start|stop|status}");
 	}
@@ -129,6 +127,8 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
 
 PX4_MAIN_FUNCTION(mc_att_control)
 {
+	px4::init(argc, argv, "mc_att_control");
+
 	PX4_INFO("starting");
 	MulticopterAttitudeControl attctl;
 	thread_running = true;

From 2c12a524de2c16b33722d2d128515cfd21367b4d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 29 Dec 2014 11:57:45 +0100
Subject: [PATCH 197/416] dummy estimator skeleton code

---
 CMakeLists.txt                                |  9 +++
 .../ros/nodes/att_estimator/att_estimator.cpp | 61 +++++++++++++++++++
 2 files changed, 70 insertions(+)
 create mode 100644 src/platforms/ros/nodes/att_estimator/att_estimator.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 519afa0e29..5e0f9b29c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -173,6 +173,15 @@ target_link_libraries(mc_att_control
   px4
 )
 
+## Attitude Estimator dummy
+add_executable(att_estimator
+	src/platforms/ros/nodes/att_estimator/att_estimator.cpp)
+add_dependencies(att_estimator px4_generate_messages_cpp)
+target_link_libraries(att_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
 
 #############
 ## Install ##
diff --git a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp
new file mode 100644
index 0000000000..986412a1d5
--- /dev/null
+++ b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.cpp
+ * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the ROS system.
+ */
+void chatterCallback(const std_msgs::String::ConstPtr& msg)
+{
+  ROS_INFO("I heard: [%s]", msg->data.c_str());
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "att_estimator");
+  ros::NodeHandle n;
+  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
+
+  ros::spin();
+
+  return 0;
+}

From 3e941a2d1f4919f05beee0672211efb35bfb816d Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Mon, 29 Dec 2014 14:03:37 +0100
Subject: [PATCH 198/416] progress on att_estimator node

---
 src/platforms/ros/nodes/att_estimator/att_estimator.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp
index 986412a1d5..fa021fff01 100644
--- a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp
+++ b/src/platforms/ros/nodes/att_estimator/att_estimator.cpp
@@ -39,14 +39,15 @@
 */
 
 #include "ros/ros.h"
-#include "std_msgs/String.h"
+#include <gazebo_msgs/ModelStates.h>
+//#include "std_msgs/String.h"
 
 /**
  * This tutorial demonstrates simple receipt of messages over the ROS system.
  */
-void chatterCallback(const std_msgs::String::ConstPtr& msg)
+void chatterCallback(const gazebo_msgs::ModelStates &msg)
 {
-  ROS_INFO("I heard: [%s]", msg->data.c_str());
+  // try to read out message here
 }
 
 int main(int argc, char **argv)

From 1c6da49e3f2ea805d6b0a228dd59d95a24d70d9c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 29 Dec 2014 14:11:33 +0100
Subject: [PATCH 199/416] add gazebo_msgs

---
 CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5e0f9b29c9..7893c28b1e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   message_generation
   cmake_modules
+  gazebo_msgs
 )
 find_package(Eigen REQUIRED)
 
@@ -84,6 +85,7 @@ add_message_files(
 generate_messages(
   DEPENDENCIES
   std_msgs
+  gazebo_msgs
 )
 
 ###################################

From c9b0dfaaa118e6ec42fb6384e35a40299a078079 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 08:17:59 +0100
Subject: [PATCH 200/416] skeleton code for manual input node

---
 CMakeLists.txt                                | 10 +++
 launch/multicopter.launch                     |  2 +
 .../ros/nodes/manual_input/manual_input.cpp   | 71 +++++++++++++++++++
 .../ros/nodes/manual_input/manual_input.h     | 61 ++++++++++++++++
 4 files changed, 144 insertions(+)
 create mode 100644 src/platforms/ros/nodes/manual_input/manual_input.cpp
 create mode 100644 src/platforms/ros/nodes/manual_input/manual_input.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7893c28b1e..2588680270 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   cmake_modules
   gazebo_msgs
+  sensor_msgs
 )
 find_package(Eigen REQUIRED)
 
@@ -184,6 +185,15 @@ target_link_libraries(att_estimator
   px4
 )
 
+## Manual input
+add_executable(manual_input
+	src/platforms/ros/nodes/manual_input/manual_input.cpp)
+add_dependencies(manual_input px4_generate_messages_cpp)
+target_link_libraries(manual_input
+  ${catkin_LIBRARIES}
+  px4
+)
+
 
 #############
 ## Install ##
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 57b7653d50..5db0e6415d 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -1,6 +1,8 @@
 <launch>
 
 <group ns="px4_example">
+	<node pkg="px4" name="manual_input" type="manual_input"/>
+	<node pkg="px4" name="att_estimator" type="att_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 </group>
 
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
new file mode 100644
index 0000000000..531ffa6407
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manual_input.cpp
+ * Reads from the ros joystick topic and publishes to the px4 manual control input topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "manual_input.h"
+
+#include <px4/manual_control_setpoint.h>
+
+ManualInput::ManualInput() :
+	_n(),
+	_sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1000))
+{
+}
+
+
+void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
+{
+	px4::manual_control_setpoint msg_out;
+
+	/* Fill px4 manual control setpoint topic with contents from ros joystick */
+	//XXX
+
+	_man_ctrl_sp_pub.publish(msg_out);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "manual_input");
+  ManualInput m;
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
new file mode 100644
index 0000000000..1721d5e818
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manual_input.h
+ * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <sensor_msgs/Joy.h>
+
+class ManualInput {
+public:
+	ManualInput();
+
+	~ManualInput() {}
+
+protected:
+	/**
+	 * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
+	 */
+	void JoyCallback(const sensor_msgs::JoyConstPtr& msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub_joy;
+	ros::Publisher _man_ctrl_sp_pub;
+
+
+};

From b632d128aa513c4602ec3da22ccfff937d8a5e7f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 08:22:18 +0100
Subject: [PATCH 201/416] add ros helper nodes readme

---
 src/platforms/ros/nodes/README.md | 4 ++++
 1 file changed, 4 insertions(+)
 create mode 100644 src/platforms/ros/nodes/README.md

diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md
new file mode 100644
index 0000000000..277f57bfdf
--- /dev/null
+++ b/src/platforms/ros/nodes/README.md
@@ -0,0 +1,4 @@
+# PX4 Nodes
+
+This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in
+ROS. They act as a bridge between the PX4 specific topics and the ROS topics.

From 1f8fd5d1207512711c47859e06ac23c340be1551 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 09:13:20 +0100
Subject: [PATCH 202/416] new dummy attitude estimator skeleton

---
 CMakeLists.txt                                |  8 +--
 launch/multicopter.launch                     |  2 +-
 .../attitude_estimator/attitude_estimator.cpp | 70 +++++++++++++++++++
 .../attitude_estimator.h}                     | 33 +++++----
 .../ros/nodes/manual_input/manual_input.cpp   |  3 +-
 5 files changed, 92 insertions(+), 24 deletions(-)
 create mode 100644 src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
 rename src/platforms/ros/nodes/{att_estimator/att_estimator.cpp => attitude_estimator/attitude_estimator.h} (81%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2588680270..d506cadb17 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -177,10 +177,10 @@ target_link_libraries(mc_att_control
 )
 
 ## Attitude Estimator dummy
-add_executable(att_estimator
-	src/platforms/ros/nodes/att_estimator/att_estimator.cpp)
-add_dependencies(att_estimator px4_generate_messages_cpp)
-target_link_libraries(att_estimator
+add_executable(attitude_estimator
+	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+add_dependencies(attitude_estimator px4_generate_messages_cpp)
+target_link_libraries(attitude_estimator
   ${catkin_LIBRARIES}
   px4
 )
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 5db0e6415d..8cc89450dd 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -2,7 +2,7 @@
 
 <group ns="px4_example">
 	<node pkg="px4" name="manual_input" type="manual_input"/>
-	<node pkg="px4" name="att_estimator" type="att_estimator"/>
+	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 </group>
 
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
new file mode 100644
index 0000000000..7504091ebb
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.cpp
+ * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "attitude_estimator.h"
+
+#include <px4/vehicle_attitude.h>
+
+AttitudeEstimator::AttitudeEstimator() :
+	_n(),
+	_sub_modelstates(_n.subscribe("joy", 10, &AttitudeEstimator::ModelStatesCallback, this)),
+	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
+{
+}
+
+void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg)
+{
+	px4::vehicle_attitude msg_out;
+
+	/* Fill px4 attitude topic with contents from modelstates topic */
+	//XXX
+
+	_vehicle_attitude_pub.publish(msg_out);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "attitude_estimator");
+  AttitudeEstimator m;
+
+  ros::spin();
+
+  return 0;
+}
diff --git a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
similarity index 81%
rename from src/platforms/ros/nodes/att_estimator/att_estimator.cpp
rename to src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
index fa021fff01..e3326b7152 100644
--- a/src/platforms/ros/nodes/att_estimator/att_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file att_estimator.cpp
+ * @file att_estimator.h
  * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
@@ -40,23 +40,22 @@
 
 #include "ros/ros.h"
 #include <gazebo_msgs/ModelStates.h>
-//#include "std_msgs/String.h"
 
-/**
- * This tutorial demonstrates simple receipt of messages over the ROS system.
- */
-void chatterCallback(const gazebo_msgs::ModelStates &msg)
-{
-  // try to read out message here
-}
+class AttitudeEstimator {
+public:
+	AttitudeEstimator();
 
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "att_estimator");
-  ros::NodeHandle n;
-  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
+	~AttitudeEstimator() {}
 
-  ros::spin();
+protected:
+	/**
+	 * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
+	 */
+	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg);
 
-  return 0;
-}
+	ros::NodeHandle _n;
+	ros::Subscriber _sub_modelstates;
+	ros::Publisher _vehicle_attitude_pub;
+
+
+};
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 531ffa6407..41e597ab79 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -45,11 +45,10 @@
 ManualInput::ManualInput() :
 	_n(),
 	_sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
-	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1000))
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 10))
 {
 }
 
-
 void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 {
 	px4::manual_control_setpoint msg_out;

From 4debf45e06ada59a5675f7fa2bc180b829464c02 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 09:58:30 +0100
Subject: [PATCH 203/416] add joystick to launch file

---
 launch/multicopter.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 8cc89450dd..e54ebb3e23 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -1,6 +1,7 @@
 <launch>
 
 <group ns="px4_example">
+	<node pkg="joy" name="joy_node" type="joy_node"/>
 	<node pkg="px4" name="manual_input" type="manual_input"/>
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>

From 27203beaa7b9c9110d93951ad4c068164d96f866 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 09:58:45 +0100
Subject: [PATCH 204/416] add some info in README about joystick

---
 src/platforms/ros/nodes/README.md | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md
index 277f57bfdf..aafc647ff1 100644
--- a/src/platforms/ros/nodes/README.md
+++ b/src/platforms/ros/nodes/README.md
@@ -2,3 +2,21 @@
 
 This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in
 ROS. They act as a bridge between the PX4 specific topics and the ROS topics.
+
+## Joystick Input
+
+You will need to install the ros joystick packages
+See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick
+
+### Arch Linux
+```sh
+yaourt -Sy ros-indigo-joystick-drivers --noconfirm
+```
+check joystick
+```sh
+ls /dev/input/
+ls -l /dev/input/js0
+```
+(replace 0 by the number you find with the first command)
+
+make sure the joystick is accessible by the `input` group and that your user is in the `input` group

From 4c511210793c7b2479291bb2abb1de7ce1362b9e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 10:19:33 +0100
Subject: [PATCH 205/416] add example ROS_INFO

---
 .../ros/nodes/attitude_estimator/attitude_estimator.cpp        | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index 7504091ebb..e64ba335d3 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -44,7 +44,7 @@
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
-	_sub_modelstates(_n.subscribe("joy", 10, &AttitudeEstimator::ModelStatesCallback, this)),
+	_sub_modelstates(_n.subscribe("gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
 	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
 {
 }
@@ -54,6 +54,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	px4::vehicle_attitude msg_out;
 
 	/* Fill px4 attitude topic with contents from modelstates topic */
+	ROS_INFO("Test x: %.4f", msg->pose[0].orientation.x);
 	//XXX
 
 	_vehicle_attitude_pub.publish(msg_out);

From 67c49303150b61403ad49018f617d870b703221e Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 30 Dec 2014 10:21:21 +0100
Subject: [PATCH 206/416] added mixer node

---
 src/platforms/ros/nodes/mc_mixer.cpp | 72 ++++++++++++++++++++++++++++
 1 file changed, 72 insertions(+)
 create mode 100644 src/platforms/ros/nodes/mc_mixer.cpp

diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer.cpp
new file mode 100644
index 0000000000..ddafde3b96
--- /dev/null
+++ b/src/platforms/ros/nodes/mc_mixer.cpp
@@ -0,0 +1,72 @@
+e/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.cpp
+ * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+ #include "ros/ros.h"
+
+ struct {
+		float control[8];
+	}inputs;
+
+	struct  {
+		float control[8];
+	}outputs;
+
+void mix(void *input) {
+
+
+}
+
+ void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
+{
+  	// read message
+	memcpy(inputs,msg,sizeof(inputs));
+	// mix
+
+}
+
+ int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mc_mixer");
+  ros::NodeHandle n;
+  ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback);
+
+  ros::spin();
+
+  return 0;
+}

From 484020177d947bf308466cb0bd5342cee0dd014a Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 30 Dec 2014 11:06:40 +0100
Subject: [PATCH 207/416] further progress on mixer node

---
 CMakeLists.txt                       |   9 +++
 src/platforms/ros/nodes/mc_mixer.cpp | 101 +++++++++++++++++++++++++--
 2 files changed, 106 insertions(+), 4 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5e0f9b29c9..57d1bc440f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -182,6 +182,15 @@ target_link_libraries(att_estimator
   px4
 )
 
+## Multicopter Mixer dummy
+add_executable(mc_mixer
+  src/platforms/ros/nodes/mc_mixer.cpp)
+add_dependencies(mc_mixer px4_generate_messages_cpp)
+target_link_libraries(mc_mixer
+  ${catkin_LIBRARIES}
+  px4
+)
+
 
 #############
 ## Install ##
diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer.cpp
index ddafde3b96..a131440a89 100644
--- a/src/platforms/ros/nodes/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer.cpp
@@ -1,4 +1,4 @@
-e/****************************************************************************
+/****************************************************************************
  *
  *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
@@ -37,7 +37,19 @@ e/****************************************************************************
  *
  * @author Roman Bapst <romanbapst@yahoo.de>
 */
- #include "ros/ros.h"
+ #include <ros/ros.h>
+ #include <px4.h>
+ #include <lib/mathlib/mathlib.h>
+ #include <mav_msgs/MotorSpeed.h>
+
+ #define _rotor_count 4
+
+ struct Rotor {
+	float roll_scale;
+	float pitch_scale;
+	float yaw_scale;
+};
+
 
  struct {
 		float control[8];
@@ -47,16 +59,96 @@ e/****************************************************************************
 		float control[8];
 	}outputs;
 
-void mix(void *input) {
+
+const Rotor _config_x[] = {
+	{  0.000000,  -1.000000, -1.00 },
+	{ -0.000000, 1.000000, -1.00 },
+	{  1.000000,  0.000000,  1.00 },
+	{  -1.000000,  0.000000,  1.00 },
+};
+
+	const Rotor *_rotors = { &_config_x[0]
+
+	};
 
 
+void mix() {
+	float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
+	float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
+	float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f);
+	float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f);
+	float min_out = 0.0f;
+	float max_out = 0.0f;
+
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float out = roll * _rotors[i].roll_scale
+				+ pitch * _rotors[i].pitch_scale + thrust;
+
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = -out / _rotors[i].yaw_scale;
+		}
+
+		/* calculate min and max output values */
+		if (out < min_out) {
+			min_out = out;
+		}
+		if (out > max_out) {
+			max_out = out;
+		}
+
+		outputs.control[i] = out;
+	}
+	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+	if (min_out < 0.0f) {
+		float scale_in = thrust / (thrust - min_out);
+
+		/* mix again with adjusted controls */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] = scale_in
+					* (roll * _rotors[i].roll_scale
+							+ pitch * _rotors[i].pitch_scale) + thrust;
+		}
+
+	} else {
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] += yaw * _rotors[i].yaw_scale;
+		}
+	}
+
+	/* scale down all outputs if some outputs are too large, reduce total thrust */
+	float scale_out;
+	if (max_out > 1.0f) {
+		scale_out = 1.0f / max_out;
+
+	} else {
+		scale_out = 1.0f;
+	}
+
+	/* scale outputs to range _idle_speed..1, and do final limiting */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f);
+	}
 }
 
  void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
 {
   	// read message
-	memcpy(inputs,msg,sizeof(inputs));
+	for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) {
+		inputs.control[i] = msg.control[i];
+	}
+	
 	// mix
+	mix();
+
+	// publish message
+	mav_msgs::MotorSpeed rotor_vel_msg;
+	for (int i = 0; i < _rotor_count; i++) {
+    	rotor_vel_msg.motor_speed.push_back(outputs.control[i]);
+	}
+	pub.publish(rotor_vel_msg);
 
 }
 
@@ -65,6 +157,7 @@ void mix(void *input) {
   ros::init(argc, argv, "mc_mixer");
   ros::NodeHandle n;
   ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback);
+  ros::Publisher pub = n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
 
   ros::spin();
 

From 824446bf2f91d76743d9c8c17d710d4ac51bfaf8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 11:15:46 +0100
Subject: [PATCH 208/416] typo

---
 src/platforms/px4_defines.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index c1a0782c49..d6d23d013a 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -78,7 +78,7 @@
 /* Parameter handle datatype */
 typedef const char *px4_param_t;
 
-/* Helper fucntions to set ROS params, only int and float supported */
+/* Helper functions to set ROS params, only int and float supported */
 static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
 {
 	ros::param::set(name, value);

From c01680459409d0119e72becc48d497fdf59e7023 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 11:16:53 +0100
Subject: [PATCH 209/416] manual input: map axes

---
 .../ros/nodes/manual_input/manual_input.cpp   | 27 ++++++++++++++++++-
 .../ros/nodes/manual_input/manual_input.h     | 10 +++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 41e597ab79..8fc1585046 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -47,6 +47,22 @@ ManualInput::ManualInput() :
 	_sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
 	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 10))
 {
+	/* Load parameters, default values work for Microsoft XBox Controller */
+	_n.param("map_x", param_axes_map[0], 4);
+	_n.param("scale_x", param_axes_scale[0], 1.0);
+	_n.param("off_x", param_axes_offset[0], 0.0);
+
+	_n.param("map_y", param_axes_map[1], 3);
+	_n.param("scale_y", param_axes_scale[1], -1.0);
+	_n.param("off_y", param_axes_offset[1], 0.0);
+
+	_n.param("map_z", param_axes_map[2], 2);
+	_n.param("scale_z", param_axes_scale[2], -0.5);
+	_n.param("off_z", param_axes_offset[2], 0.5);
+
+	_n.param("map_r", param_axes_map[3], 0);
+	_n.param("scale_r", param_axes_scale[3], -1.0);
+	_n.param("off_r", param_axes_offset[3], 0.0);
 }
 
 void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
@@ -54,11 +70,20 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 	px4::manual_control_setpoint msg_out;
 
 	/* Fill px4 manual control setpoint topic with contents from ros joystick */
-	//XXX
+	MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x);
+	MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y);
+	MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z);
+	MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r);
+	ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
 
 	_man_ctrl_sp_pub.publish(msg_out);
 }
 
+void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out)
+{
+	out = (float)(msg->axes[map_index] * scale + offset);
+}
+
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "manual_input");
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index 1721d5e818..fbe635cf2a 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -53,9 +53,19 @@ protected:
 	 */
 	void JoyCallback(const sensor_msgs::JoyConstPtr& msg);
 
+	/**
+	 * Helper function to map and scale joystick input
+	 */
+	void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _sub_joy;
 	ros::Publisher _man_ctrl_sp_pub;
 
+	/* Parameters */
+	int param_axes_map[4];
+	double param_axes_scale[4];
+	double param_axes_offset[4];
+
 
 };

From 502952e034624e789755368b6a8ab6f8b88ba262 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 11:25:27 +0100
Subject: [PATCH 210/416] make clear that switches are hardcoded to manual mode

---
 .../ros/nodes/manual_input/manual_input.cpp         | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 8fc1585046..2ee2dadbc8 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -70,11 +70,22 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 	px4::manual_control_setpoint msg_out;
 
 	/* Fill px4 manual control setpoint topic with contents from ros joystick */
+	/* Map sticks to x, y, z, r */
 	MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x);
 	MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y);
 	MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z);
 	MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r);
-	ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
+	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
+
+	/* Map buttons to switches */
+	//XXX todo
+	/* for now just publish switches in position for manual flight */
+	msg_out.mode_switch = 0;
+	msg_out.return_switch = 0;
+	msg_out.posctl_switch = 0;
+	msg_out.loiter_switch = 0;
+	msg_out.acro_switch = 0;
+	msg_out.offboard_switch = 0;
 
 	_man_ctrl_sp_pub.publish(msg_out);
 }

From fa1f09b850f754ff3e0da7f4c5e56e3ee58fce11 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 30 Dec 2014 11:34:37 +0100
Subject: [PATCH 211/416] made class for mc_mixer and moved into folder

---
 .../ros/nodes/{ => mc_mixer}/mc_mixer.cpp     | 61 +++++++++++++------
 1 file changed, 44 insertions(+), 17 deletions(-)
 rename src/platforms/ros/nodes/{ => mc_mixer}/mc_mixer.cpp (81%)

diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
similarity index 81%
rename from src/platforms/ros/nodes/mc_mixer.cpp
rename to src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index a131440a89..79eaf19e52 100644
--- a/src/platforms/ros/nodes/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -41,38 +41,67 @@
  #include <px4.h>
  #include <lib/mathlib/mathlib.h>
  #include <mav_msgs/MotorSpeed.h>
+ 
+ class MultirotorMixer {
+public:
 
- #define _rotor_count 4
+	MultirotorMixer();
 
- struct Rotor {
-	float roll_scale;
-	float pitch_scale;
-	float yaw_scale;
-};
+	struct Rotor {
+			float roll_scale;
+			float pitch_scale;
+			float yaw_scale;
 
+		};
 
- struct {
-		float control[8];
+	void mix();
+	void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
+
+private:
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub;
+	ros::Publisher _pub;
+	
+	const Rotor *_rotors;
+
+	unsigned _rotor_count;
+
+	struct {
+		float control[6];
 	}inputs;
 
 	struct  {
-		float control[8];
+		float control[6];
 	}outputs;
 
 
-const Rotor _config_x[] = {
+		
+};
+
+ 
+
+const MultirotorMixer::Rotor _config_x[] = {
 	{  0.000000,  -1.000000, -1.00 },
 	{ -0.000000, 1.000000, -1.00 },
 	{  1.000000,  0.000000,  1.00 },
 	{  -1.000000,  0.000000,  1.00 },
 };
 
-	const Rotor *_rotors = { &_config_x[0]
+	const MultirotorMixer::Rotor *_config_index = { &_config_x[0]
 
 	};
 
+MultirotorMixer::MultirotorMixer():
+	_rotor_count(4),
+	_rotors(_config_index)
+{
+	_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
+  	_pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
+}
 
-void mix() {
+
+void MultirotorMixer::mix() {
 	float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
 	float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
 	float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f);
@@ -133,7 +162,7 @@ void mix() {
 	}
 }
 
- void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
+ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
 {
   	// read message
 	for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) {
@@ -148,16 +177,14 @@ void mix() {
 	for (int i = 0; i < _rotor_count; i++) {
     	rotor_vel_msg.motor_speed.push_back(outputs.control[i]);
 	}
-	pub.publish(rotor_vel_msg);
+	_pub.publish(rotor_vel_msg);
 
 }
 
  int main(int argc, char **argv)
 {
   ros::init(argc, argv, "mc_mixer");
-  ros::NodeHandle n;
-  ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback);
-  ros::Publisher pub = n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
+  MultirotorMixer mixer;  
 
   ros::spin();
 

From bfc398442663a943ff1e0fa21ec50b9960abb5a7 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 30 Dec 2014 11:41:28 +0100
Subject: [PATCH 212/416] cleanup

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 38 ++++++++-----------
 1 file changed, 15 insertions(+), 23 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 79eaf19e52..7092a80175 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -41,20 +41,18 @@
  #include <px4.h>
  #include <lib/mathlib/mathlib.h>
  #include <mav_msgs/MotorSpeed.h>
- 
+
  class MultirotorMixer {
 public:
 
 	MultirotorMixer();
 
 	struct Rotor {
-			float roll_scale;
-			float pitch_scale;
-			float yaw_scale;
+		float roll_scale;
+		float pitch_scale;
+		float yaw_scale;
+	};
 
-		};
-
-	void mix();
 	void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
 
 private:
@@ -62,7 +60,7 @@ private:
 	ros::NodeHandle _n;
 	ros::Subscriber _sub;
 	ros::Publisher _pub;
-	
+
 	const Rotor *_rotors;
 
 	unsigned _rotor_count;
@@ -75,21 +73,18 @@ private:
 		float control[6];
 	}outputs;
 
+	void mix();
 
-		
 };
 
- 
-
 const MultirotorMixer::Rotor _config_x[] = {
-	{  0.000000,  -1.000000, -1.00 },
-	{ -0.000000, 1.000000, -1.00 },
+	{  0.000000, -1.000000, -1.00 },
+	{ -0.000000,  1.000000, -1.00 },
 	{  1.000000,  0.000000,  1.00 },
-	{  -1.000000,  0.000000,  1.00 },
+	{ -1.000000,  0.000000,  1.00 },
 };
 
 	const MultirotorMixer::Rotor *_config_index = { &_config_x[0]
-
 	};
 
 MultirotorMixer::MultirotorMixer():
@@ -97,10 +92,9 @@ MultirotorMixer::MultirotorMixer():
 	_rotors(_config_index)
 {
 	_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
-  	_pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
+	_pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
 }
 
-
 void MultirotorMixer::mix() {
 	float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
 	float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
@@ -164,28 +158,26 @@ void MultirotorMixer::mix() {
 
  void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
 {
-  	// read message
+	// read message
 	for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) {
 		inputs.control[i] = msg.control[i];
 	}
-	
+
 	// mix
 	mix();
 
 	// publish message
 	mav_msgs::MotorSpeed rotor_vel_msg;
 	for (int i = 0; i < _rotor_count; i++) {
-    	rotor_vel_msg.motor_speed.push_back(outputs.control[i]);
+		rotor_vel_msg.motor_speed.push_back(outputs.control[i]);
 	}
 	_pub.publish(rotor_vel_msg);
-
 }
 
  int main(int argc, char **argv)
 {
   ros::init(argc, argv, "mc_mixer");
-  MultirotorMixer mixer;  
-
+  MultirotorMixer mixer;
   ros::spin();
 
   return 0;

From 4307b48c03779a196f8b16697ac91213f4b39792 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 30 Dec 2014 11:46:18 +0100
Subject: [PATCH 213/416] fixed description

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 7092a80175..1e5ef99a34 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file att_estimator.cpp
- * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ * @file mc_mixer.cpp
+ * Dummy multicopter mixer for euroc simulator (gazebo)
  *
  * @author Roman Bapst <romanbapst@yahoo.de>
 */

From 6f425ca7fcaa12f30ff475b9578473ede369d2c1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 12:27:29 +0100
Subject: [PATCH 214/416] commander dummy node

---
 CMakeLists.txt                                |  9 ++
 src/platforms/px4_middleware.h                |  4 +
 .../ros/nodes/commander/commander.cpp         | 96 +++++++++++++++++++
 src/platforms/ros/nodes/commander/commander.h | 62 ++++++++++++
 4 files changed, 171 insertions(+)
 create mode 100644 src/platforms/ros/nodes/commander/commander.cpp
 create mode 100644 src/platforms/ros/nodes/commander/commander.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d506cadb17..4c504bff4b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -194,6 +194,15 @@ target_link_libraries(manual_input
   px4
 )
 
+## Commander
+add_executable(commander
+	src/platforms/ros/nodes/commander/commander.cpp)
+add_dependencies(manual_input px4_generate_messages_cpp)
+target_link_libraries(commander
+  ${catkin_LIBRARIES}
+  px4
+)
+
 
 #############
 ## Install ##
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index a8f3ad666d..54050de8b3 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -42,6 +42,10 @@
 #include <stdint.h>
 #include <unistd.h>
 
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#define __EXPORT
+#endif
+
 namespace px4
 {
 
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
new file mode 100644
index 0000000000..f767bbb361
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.cpp
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "commander.h"
+
+#include <px4/manual_control_setpoint.h>
+#include <px4/vehicle_control_mode.h>
+#include <px4/actuator_armed.h>
+#include <px4/vehicle_status.h>
+#include <platforms/px4_middleware.h>
+
+Commander::Commander() :
+	_n(),
+	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+	_vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
+	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
+	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10))
+{
+}
+
+void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg)
+{
+	px4::vehicle_control_mode msg_vehicle_control_mode;
+	px4::actuator_armed msg_actuator_armed;
+	px4::vehicle_status msg_vehicle_status;
+
+	/* fill vehicle control mode */
+	//XXX hardcoded
+	msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+	msg_vehicle_control_mode.flag_control_manual_enabled = true;
+	msg_vehicle_control_mode.flag_control_rates_enabled = true;
+	msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+
+	/* fill actuator armed */
+	//XXX hardcoded
+	msg_actuator_armed.timestamp = px4::get_time_micros();
+	msg_actuator_armed.armed = true;
+
+	/* fill vehicle status */
+	//XXX hardcoded
+	msg_vehicle_status.timestamp = px4::get_time_micros();
+	msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
+	msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
+	msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+	msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
+	msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+
+	_vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
+	_actuator_armed_pub.publish(msg_actuator_armed);
+	_vehicle_status_pub.publish(msg_vehicle_status);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "commander");
+  Commander m;
+  ros::spin();
+  return 0;
+}
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
new file mode 100644
index 0000000000..d7fe0a4caa
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.h
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+
+class Commander {
+public:
+	Commander();
+
+	~Commander() {}
+
+protected:
+	/**
+	 * Based on manual control input the status will be set
+	 */
+	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Publisher _vehicle_control_mode_pub;
+	ros::Publisher _actuator_armed_pub;
+	ros::Publisher _vehicle_status_pub;
+
+};

From 838f8603475def949646ececc8059e6b924bda28 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 12:27:45 +0100
Subject: [PATCH 215/416] variable rename

---
 src/platforms/ros/nodes/manual_input/manual_input.cpp | 7 ++++---
 src/platforms/ros/nodes/manual_input/manual_input.h   | 2 +-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 2ee2dadbc8..b125bb1806 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -41,10 +41,11 @@
 #include "manual_input.h"
 
 #include <px4/manual_control_setpoint.h>
+#include <platforms/px4_middleware.h>
 
 ManualInput::ManualInput() :
 	_n(),
-	_sub_joy(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
+	joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
 	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 10))
 {
 	/* Load parameters, default values work for Microsoft XBox Controller */
@@ -87,6 +88,8 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 	msg_out.acro_switch = 0;
 	msg_out.offboard_switch = 0;
 
+	msg_out.timestamp = px4::get_time_micros();
+
 	_man_ctrl_sp_pub.publish(msg_out);
 }
 
@@ -99,8 +102,6 @@ int main(int argc, char **argv)
 {
   ros::init(argc, argv, "manual_input");
   ManualInput m;
-
   ros::spin();
-
   return 0;
 }
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index fbe635cf2a..22a158985e 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -59,7 +59,7 @@ protected:
 	void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out);
 
 	ros::NodeHandle _n;
-	ros::Subscriber _sub_joy;
+	ros::Subscriber joy_sub;
 	ros::Publisher _man_ctrl_sp_pub;
 
 	/* Parameters */

From f968e7355d62bbf8df426c51dc2aba98d2bc72b7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 12:31:24 +0100
Subject: [PATCH 216/416] fix mixer path

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a7277b02a0..220cd762bb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -196,7 +196,7 @@ target_link_libraries(manual_input
 
 ## Multicopter Mixer dummy
 add_executable(mc_mixer
-  src/platforms/ros/nodes/mc_mixer.cpp)
+  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
 add_dependencies(mc_mixer px4_generate_messages_cpp)
 target_link_libraries(mc_mixer
   ${catkin_LIBRARIES}

From 16c66669c257767173d52febd0f526c0e718e004 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 14:53:53 +0100
Subject: [PATCH 217/416] dummy attitude estimator copies attitude from gazebo

---
 src/lib/mathlib/math/Matrix.hpp               |  1 +
 src/lib/mathlib/math/Vector.hpp               |  2 ++
 .../attitude_estimator/attitude_estimator.cpp | 31 ++++++++++++++++---
 3 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 1e76aae603..ddadf707c3 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -52,6 +52,7 @@
 #include <platforms/ros/eigen_math.h>
 #include <Eigen/Eigen>
 #endif
+#include <platforms/px4_defines.h>
 
 namespace math
 {
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 20f099831a..781c14d539 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -52,6 +52,8 @@
 #include <platforms/ros/eigen_math.h>
 #endif
 
+#include <platforms/px4_defines.h>
+
 namespace math
 {
 
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index e64ba335d3..99d1ae0241 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -33,14 +33,16 @@
 
 /**
  * @file att_estimator.cpp
- * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Roman Bapst <romanbapst@yahoo.de>
 */
 
 #include "attitude_estimator.h"
 
 #include <px4/vehicle_attitude.h>
+#include <mathlib/mathlib.h>
+#include <platforms/px4_defines.h>
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
@@ -51,13 +53,32 @@ AttitudeEstimator::AttitudeEstimator() :
 
 void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg)
 {
-	px4::vehicle_attitude msg_out;
+	px4::vehicle_attitude msg_v_att;
 
 	/* Fill px4 attitude topic with contents from modelstates topic */
-	ROS_INFO("Test x: %.4f", msg->pose[0].orientation.x);
-	//XXX
 
-	_vehicle_attitude_pub.publish(msg_out);
+	/* Convert quaternion to rotation matrix */
+	math::Quaternion quat;
+	quat(0) = (float)msg->pose[0].orientation.w;
+	quat(1) = (float)msg->pose[0].orientation.x;
+	quat(2) = (float)msg->pose[0].orientation.y;
+	quat(3) = (float)msg->pose[0].orientation.z;
+
+	msg_v_att.q[0] = quat(0);
+	msg_v_att.q[1] = quat(1);
+	msg_v_att.q[2] = quat(2);
+	msg_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> Rot = quat.to_dcm();
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(msg_v_att.R, i, j) = Rot(i, j);
+		}
+	}
+
+	msg_v_att.R_valid = true;
+
+	_vehicle_attitude_pub.publish(msg_v_att);
 }
 
 int main(int argc, char **argv)

From 481f18cb3e20ed74b03dc02c919648f7a15b99e6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 14:58:00 +0100
Subject: [PATCH 218/416] include commander and mixer in launch file

---
 launch/multicopter.launch | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index e54ebb3e23..e7f49319cb 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -3,6 +3,8 @@
 <group ns="px4_example">
 	<node pkg="joy" name="joy_node" type="joy_node"/>
 	<node pkg="px4" name="manual_input" type="manual_input"/>
+	<node pkg="px4" name="commander" type="commander"/>
+	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 </group>

From 6f446d9abcf6eaf057183f64f7dbeb1bc75dbc26 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 15:31:44 +0100
Subject: [PATCH 219/416] add first version of simulator launch file

---
 launch/gazebo_multicopter.launch | 13 +++++++++++++
 1 file changed, 13 insertions(+)
 create mode 100644 launch/gazebo_multicopter.launch

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch
new file mode 100644
index 0000000000..c01ad163a1
--- /dev/null
+++ b/launch/gazebo_multicopter.launch
@@ -0,0 +1,13 @@
+<launch>
+
+<group ns="px4_example">
+	<include file="$(find mav_gazebo)/launch/vtol_empty_world.launch" />
+	<node pkg="joy" name="joy_node" type="joy_node"/>
+	<node pkg="px4" name="manual_input" type="manual_input"/>
+	<node pkg="px4" name="commander" type="commander"/>
+	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
+	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
+	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
+</group>
+
+</launch>

From 133842e89d83bc9a4dd0523263bea6e855caf08e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 15:43:23 +0100
Subject: [PATCH 220/416] fix launch files

---
 launch/gazebo_multicopter.launch | 4 ++--
 launch/multicopter.launch        | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch
index c01ad163a1..263ee025b1 100644
--- a/launch/gazebo_multicopter.launch
+++ b/launch/gazebo_multicopter.launch
@@ -1,7 +1,7 @@
 <launch>
+<include file="$(find mav_gazebo)/launch/vtol_empty_world.launch" />
 
-<group ns="px4_example">
-	<include file="$(find mav_gazebo)/launch/vtol_empty_world.launch" />
+<group ns="px4_multicopter">
 	<node pkg="joy" name="joy_node" type="joy_node"/>
 	<node pkg="px4" name="manual_input" type="manual_input"/>
 	<node pkg="px4" name="commander" type="commander"/>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index e7f49319cb..96ff3ad99c 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -1,6 +1,6 @@
 <launch>
 
-<group ns="px4_example">
+<group ns="px4_multicopter">
 	<node pkg="joy" name="joy_node" type="joy_node"/>
 	<node pkg="px4" name="manual_input" type="manual_input"/>
 	<node pkg="px4" name="commander" type="commander"/>

From 0ea60f56e9f52ea2ee5eac41183fea09720d9e7c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 15:55:39 +0100
Subject: [PATCH 221/416] attitude estimator: fix readin of pose

---
 .../nodes/attitude_estimator/attitude_estimator.cpp   | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index 99d1ae0241..bde436aca8 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -46,7 +46,7 @@
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
-	_sub_modelstates(_n.subscribe("gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
+	_sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
 	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
 {
 }
@@ -59,10 +59,11 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 
 	/* Convert quaternion to rotation matrix */
 	math::Quaternion quat;
-	quat(0) = (float)msg->pose[0].orientation.w;
-	quat(1) = (float)msg->pose[0].orientation.x;
-	quat(2) = (float)msg->pose[0].orientation.y;
-	quat(3) = (float)msg->pose[0].orientation.z;
+	//XXX: search for vtol or other (other than 'plane') vehicle here
+	quat(0) = (float)msg->pose[1].orientation.w;
+	quat(1) = (float)msg->pose[1].orientation.x;
+	quat(2) = (float)msg->pose[1].orientation.y;
+	quat(3) = (float)msg->pose[1].orientation.z;
 
 	msg_v_att.q[0] = quat(0);
 	msg_v_att.q[1] = quat(1);

From 60b02477e5ae72c6afc6fd609e28c6e74c955060 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 16:01:52 +0100
Subject: [PATCH 222/416] fix mixed motor commands publication

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 1e5ef99a34..f92712c552 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -92,7 +92,7 @@ MultirotorMixer::MultirotorMixer():
 	_rotors(_config_index)
 {
 	_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
-	_pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
+	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);
 }
 
 void MultirotorMixer::mix() {

From a2edc284b2bc0d5e462e9732bb1adcfb6b0a59a3 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 30 Dec 2014 16:47:38 +0100
Subject: [PATCH 223/416] fixed launch file

---
 launch/gazebo_multicopter.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch
index 263ee025b1..9c0e96e049 100644
--- a/launch/gazebo_multicopter.launch
+++ b/launch/gazebo_multicopter.launch
@@ -1,5 +1,5 @@
 <launch>
-<include file="$(find mav_gazebo)/launch/vtol_empty_world.launch" />
+<include file="$(find mav_gazebo)/launch/vtol_empty_world_with_joy.launch" />
 
 <group ns="px4_multicopter">
 	<node pkg="joy" name="joy_node" type="joy_node"/>

From dbaf6af257ca5811d779859a9ba4067758f07eec Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 16:55:34 +0100
Subject: [PATCH 224/416] scale dummy mixer output

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index f92712c552..69d69dcf81 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -169,7 +169,7 @@ void MultirotorMixer::mix() {
 	// publish message
 	mav_msgs::MotorSpeed rotor_vel_msg;
 	for (int i = 0; i < _rotor_count; i++) {
-		rotor_vel_msg.motor_speed.push_back(outputs.control[i]);
+		rotor_vel_msg.motor_speed.push_back(outputs.control[i] * 1000);
 	}
 	_pub.publish(rotor_vel_msg);
 }

From a0260b6f6ebe7a4709a55818c324145f17937145 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Dec 2014 16:56:03 +0100
Subject: [PATCH 225/416] use joystick launch file (joystick will not be used
 actually from euroc directly)

---
 launch/gazebo_multicopter.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch
index 263ee025b1..9c0e96e049 100644
--- a/launch/gazebo_multicopter.launch
+++ b/launch/gazebo_multicopter.launch
@@ -1,5 +1,5 @@
 <launch>
-<include file="$(find mav_gazebo)/launch/vtol_empty_world.launch" />
+<include file="$(find mav_gazebo)/launch/vtol_empty_world_with_joy.launch" />
 
 <group ns="px4_multicopter">
 	<node pkg="joy" name="joy_node" type="joy_node"/>

From bc5fe30270d450ae6e22f69b2fd6b7ff252e8bb1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 09:14:59 +0100
Subject: [PATCH 226/416] make mixer to radps scaling a param and raise default

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 69d69dcf81..61ee13ecde 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -74,7 +74,6 @@ private:
 	}outputs;
 
 	void mix();
-
 };
 
 const MultirotorMixer::Rotor _config_x[] = {
@@ -88,11 +87,15 @@ const MultirotorMixer::Rotor _config_x[] = {
 	};
 
 MultirotorMixer::MultirotorMixer():
+	_n(),
 	_rotor_count(4),
 	_rotors(_config_index)
 {
 	_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);
+	if (!_n.hasParam("motor_scaling_radps")) {
+		_n.setParam("motor_scaling_radps", 2000.0);
+	}
 }
 
 void MultirotorMixer::mix() {
@@ -168,8 +171,10 @@ void MultirotorMixer::mix() {
 
 	// publish message
 	mav_msgs::MotorSpeed rotor_vel_msg;
+	double scaling;
+	_n.getParamCached("motor_scaling_radps", scaling);
 	for (int i = 0; i < _rotor_count; i++) {
-		rotor_vel_msg.motor_speed.push_back(outputs.control[i] * 1000);
+		rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling);
 	}
 	_pub.publish(rotor_vel_msg);
 }

From f8214f3e0162fcbd0ca9382b0d00bdab2f5863ee Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 09:19:57 +0100
Subject: [PATCH 227/416] manual input fix variable names

---
 .../ros/nodes/manual_input/manual_input.cpp   | 34 +++++++++----------
 .../ros/nodes/manual_input/manual_input.h     |  8 ++---
 2 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index b125bb1806..bd20170339 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -45,25 +45,25 @@
 
 ManualInput::ManualInput() :
 	_n(),
-	joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
+	_joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
 	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 10))
 {
 	/* Load parameters, default values work for Microsoft XBox Controller */
-	_n.param("map_x", param_axes_map[0], 4);
-	_n.param("scale_x", param_axes_scale[0], 1.0);
-	_n.param("off_x", param_axes_offset[0], 0.0);
+	_n.param("map_x", _param_axes_map[0], 4);
+	_n.param("scale_x", _param_axes_scale[0], 1.0);
+	_n.param("off_x", _param_axes_offset[0], 0.0);
 
-	_n.param("map_y", param_axes_map[1], 3);
-	_n.param("scale_y", param_axes_scale[1], -1.0);
-	_n.param("off_y", param_axes_offset[1], 0.0);
+	_n.param("map_y", _param_axes_map[1], 3);
+	_n.param("scale_y", _param_axes_scale[1], -1.0);
+	_n.param("off_y", _param_axes_offset[1], 0.0);
 
-	_n.param("map_z", param_axes_map[2], 2);
-	_n.param("scale_z", param_axes_scale[2], -0.5);
-	_n.param("off_z", param_axes_offset[2], 0.5);
+	_n.param("map_z", _param_axes_map[2], 2);
+	_n.param("scale_z", _param_axes_scale[2], -0.5);
+	_n.param("off_z", _param_axes_offset[2], 0.5);
 
-	_n.param("map_r", param_axes_map[3], 0);
-	_n.param("scale_r", param_axes_scale[3], -1.0);
-	_n.param("off_r", param_axes_offset[3], 0.0);
+	_n.param("map_r", _param_axes_map[3], 0);
+	_n.param("scale_r", _param_axes_scale[3], -1.0);
+	_n.param("off_r", _param_axes_offset[3], 0.0);
 }
 
 void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
@@ -72,10 +72,10 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 
 	/* Fill px4 manual control setpoint topic with contents from ros joystick */
 	/* Map sticks to x, y, z, r */
-	MapAxis(msg, param_axes_map[0], param_axes_scale[0], param_axes_offset[0], msg_out.x);
-	MapAxis(msg, param_axes_map[1], param_axes_scale[1], param_axes_offset[1], msg_out.y);
-	MapAxis(msg, param_axes_map[2], param_axes_scale[2], param_axes_offset[2], msg_out.z);
-	MapAxis(msg, param_axes_map[3], param_axes_scale[3], param_axes_offset[3], msg_out.r);
+	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], msg_out.x);
+	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], msg_out.y);
+	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], msg_out.z);
+	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], msg_out.r);
 	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
 
 	/* Map buttons to switches */
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index 22a158985e..f579acbb77 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -59,13 +59,13 @@ protected:
 	void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out);
 
 	ros::NodeHandle _n;
-	ros::Subscriber joy_sub;
+	ros::Subscriber _joy_sub;
 	ros::Publisher _man_ctrl_sp_pub;
 
 	/* Parameters */
-	int param_axes_map[4];
-	double param_axes_scale[4];
-	double param_axes_offset[4];
+	int _param_axes_map[4];
+	double _param_axes_scale[4];
+	double _param_axes_offset[4];
 
 
 };

From 3684ac6bf1fdbaeb18686768d013f28ad75122ff Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 14:43:55 +0100
Subject: [PATCH 228/416] fix dummy mixer

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 25 +++++++++++++------
 1 file changed, 17 insertions(+), 8 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 61ee13ecde..04d7888fb9 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -77,24 +77,33 @@ private:
 };
 
 const MultirotorMixer::Rotor _config_x[] = {
-	{  0.000000, -1.000000, -1.00 },
-	{ -0.000000,  1.000000, -1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{ -1.000000,  0.000000,  1.00 },
+	{ -0.707107,  0.707107,  1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{  0.707107,  0.707107, -1.00 },
+	{ -0.707107, -0.707107, -1.00 },
 };
 
-	const MultirotorMixer::Rotor *_config_index = { &_config_x[0]
-	};
+const MultirotorMixer::Rotor _config_quad_plus[] = {
+	{ -1.000000,  0.000000,  1.00 },
+	{  1.000000,  0.000000,  1.00 },
+	{  0.000000,  1.000000, -1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+};
+
+const MultirotorMixer::Rotor *_config_index[3] = {
+	&_config_x[0],
+	&_config_quad_plus[0],
+};
 
 MultirotorMixer::MultirotorMixer():
 	_n(),
 	_rotor_count(4),
-	_rotors(_config_index)
+	_rotors(_config_index[1]) //XXX +config hardcoded
 {
 	_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);
 	if (!_n.hasParam("motor_scaling_radps")) {
-		_n.setParam("motor_scaling_radps", 2000.0);
+		_n.setParam("motor_scaling_radps", 1500.0);
 	}
 }
 

From 134f41c7077ea8592a55c07b1635bac6e23fe3bd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 15:23:09 +0100
Subject: [PATCH 229/416] make ros params from launch files work

---
 src/platforms/px4_defines.h | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index d6d23d013a..ef43c55b83 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -81,12 +81,16 @@ typedef const char *px4_param_t;
 /* Helper functions to set ROS params, only int and float supported */
 static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
 {
-	ros::param::set(name, value);
+	if (!ros::param::has(name)) {
+		ros::param::set(name, value);
+	}
 	return (px4_param_t)name;
 };
 static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 {
-	ros::param::set(name, value);
+	if (!ros::param::has(name)) {
+		ros::param::set(name, value);
+	}
 	return (px4_param_t)name;
 };
 

From 1b0446ed41eb6db676106debeef2f895edc30d01 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 15:23:28 +0100
Subject: [PATCH 230/416] improve launch files

---
 launch/gazebo_multicopter.launch | 11 ++---------
 launch/multicopter.launch        |  5 +++++
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch
index 9c0e96e049..febf7bdc07 100644
--- a/launch/gazebo_multicopter.launch
+++ b/launch/gazebo_multicopter.launch
@@ -1,13 +1,6 @@
 <launch>
-<include file="$(find mav_gazebo)/launch/vtol_empty_world_with_joy.launch" />
 
-<group ns="px4_multicopter">
-	<node pkg="joy" name="joy_node" type="joy_node"/>
-	<node pkg="px4" name="manual_input" type="manual_input"/>
-	<node pkg="px4" name="commander" type="commander"/>
-	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
-	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
-	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
-</group>
+<include file="$(find mav_gazebo)/launch/vtol_empty_world_with_joy.launch" />
+<include file="$(find px4)/launch/multicopter.launch" />
 
 </launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 96ff3ad99c..2ccd1338b8 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -7,6 +7,11 @@
 	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
+
+	<param name="MC_ROLL_P" type="double" value="6.0" />
+	<param name="MC_PITCH_P" type="double" value="6.0" />
+	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
+	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
 </group>
 
 </launch>

From e826187efe6f514a632ab2ef2e0183d17792c0a9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 16:50:30 +0100
Subject: [PATCH 231/416] reduce mixer queue size

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 04d7888fb9..966c5cfb7b 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -100,7 +100,7 @@ MultirotorMixer::MultirotorMixer():
 	_rotor_count(4),
 	_rotors(_config_index[1]) //XXX +config hardcoded
 {
-	_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
+	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);
 	if (!_n.hasParam("motor_scaling_radps")) {
 		_n.setParam("motor_scaling_radps", 1500.0);

From 2ab6eefc2966b615f66f3c8b366337fa293bc7a5 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 16:51:33 +0100
Subject: [PATCH 232/416] reduce manualinput queue size

---
 src/platforms/ros/nodes/manual_input/manual_input.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index bd20170339..eddacf55f1 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -45,8 +45,8 @@
 
 ManualInput::ManualInput() :
 	_n(),
-	_joy_sub(_n.subscribe("joy", 10, &ManualInput::JoyCallback, this)),
-	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 10))
+	_joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1))
 {
 	/* Load parameters, default values work for Microsoft XBox Controller */
 	_n.param("map_x", _param_axes_map[0], 4);

From a5890662c53d700832783f31477d1fa26f5b2d05 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 31 Dec 2014 16:52:00 +0100
Subject: [PATCH 233/416] attitude estimator: fix signs

---
 .../attitude_estimator/attitude_estimator.cpp  | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index bde436aca8..163b111be0 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -46,8 +46,8 @@
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
-	_sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
-	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
+	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
+	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
 {
 }
 
@@ -62,23 +62,27 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	//XXX: search for vtol or other (other than 'plane') vehicle here
 	quat(0) = (float)msg->pose[1].orientation.w;
 	quat(1) = (float)msg->pose[1].orientation.x;
-	quat(2) = (float)msg->pose[1].orientation.y;
-	quat(3) = (float)msg->pose[1].orientation.z;
+	quat(2) = (float)-msg->pose[1].orientation.y;
+	quat(3) = (float)-msg->pose[1].orientation.z;
 
 	msg_v_att.q[0] = quat(0);
 	msg_v_att.q[1] = quat(1);
 	msg_v_att.q[2] = quat(2);
 	msg_v_att.q[3] = quat(3);
 
-	math::Matrix<3, 3> Rot = quat.to_dcm();
+	math::Matrix<3, 3> rot = quat.to_dcm();
 	for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
-			PX4_R(msg_v_att.R, i, j) = Rot(i, j);
+			PX4_R(msg_v_att.R, i, j) = rot(i, j);
 		}
 	}
-
 	msg_v_att.R_valid = true;
 
+	math::Vector<3> euler = rot.to_euler();
+	msg_v_att.roll = euler(0);
+	msg_v_att.pitch = euler(1);
+	msg_v_att.yaw = euler(2);
+
 	_vehicle_attitude_pub.publish(msg_v_att);
 }
 

From 9586a8f5b11314380f1e07ef6a09bff3c430c7e6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 09:08:30 +0100
Subject: [PATCH 234/416] add missing mab_msgs dependency

---
 CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6312fa055b..cbd646556a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   cmake_modules
   gazebo_msgs
   sensor_msgs
+  mav_msgs
 )
 find_package(Eigen REQUIRED)
 

From 92729b3020afe204aebaeb14d502654ad9251e45 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 09:08:49 +0100
Subject: [PATCH 235/416] commander dummy node: publish param update at low
 freq to make other nodes update their params

---
 src/platforms/ros/nodes/commander/commander.cpp | 10 +++++++++-
 src/platforms/ros/nodes/commander/commander.h   |  4 ++++
 2 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index f767bbb361..1972d8cfa0 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -51,7 +51,9 @@ Commander::Commander() :
 	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
 	_vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
-	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10))
+	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
+	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
+	_msg_parameter_update()
 {
 }
 
@@ -85,6 +87,12 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	_vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
 	_actuator_armed_pub.publish(msg_actuator_armed);
 	_vehicle_status_pub.publish(msg_vehicle_status);
+
+	/* Fill parameter update */
+	if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) {
+		_msg_parameter_update.timestamp = px4::get_time_micros();
+		_parameter_update_pub.publish(_msg_parameter_update);
+	}
 }
 
 int main(int argc, char **argv)
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index d7fe0a4caa..cd9be51351 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -40,6 +40,7 @@
 
 #include "ros/ros.h"
 #include <px4/manual_control_setpoint.h>
+#include <px4/parameter_update.h>
 
 class Commander {
 public:
@@ -58,5 +59,8 @@ protected:
 	ros::Publisher _vehicle_control_mode_pub;
 	ros::Publisher _actuator_armed_pub;
 	ros::Publisher _vehicle_status_pub;
+	ros::Publisher _parameter_update_pub;
+
+	px4::parameter_update _msg_parameter_update;
 
 };

From f05dc608748fa4714b7bb1239dc5ca4743602f17 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 12:22:06 +0100
Subject: [PATCH 236/416] ros nodes: add mixer for euroc

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 966c5cfb7b..e8c65905f1 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -90,15 +90,23 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
 	{ -0.000000, -1.000000, -1.00 },
 };
 
+const MultirotorMixer::Rotor _config_quad_plus_euroc[] = {
+	{  0.000000,  1.000000,  1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  1.000000,  0.000000, -1.00 },
+	{ -1.000000,  0.000000, -1.00 },
+};
+
 const MultirotorMixer::Rotor *_config_index[3] = {
 	&_config_x[0],
 	&_config_quad_plus[0],
+	&_config_quad_plus_euroc[0]
 };
 
 MultirotorMixer::MultirotorMixer():
 	_n(),
 	_rotor_count(4),
-	_rotors(_config_index[1]) //XXX +config hardcoded
+	_rotors(_config_index[2]) //XXX + eurocconfig hardcoded
 {
 	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);

From 3932013777c232c8c91e34144f54f6fd2f635175 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 12:22:06 +0100
Subject: [PATCH 237/416] ros nodes: add mixer for euroc

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 966c5cfb7b..e8c65905f1 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -90,15 +90,23 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
 	{ -0.000000, -1.000000, -1.00 },
 };
 
+const MultirotorMixer::Rotor _config_quad_plus_euroc[] = {
+	{  0.000000,  1.000000,  1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  1.000000,  0.000000, -1.00 },
+	{ -1.000000,  0.000000, -1.00 },
+};
+
 const MultirotorMixer::Rotor *_config_index[3] = {
 	&_config_x[0],
 	&_config_quad_plus[0],
+	&_config_quad_plus_euroc[0]
 };
 
 MultirotorMixer::MultirotorMixer():
 	_n(),
 	_rotor_count(4),
-	_rotors(_config_index[1]) //XXX +config hardcoded
+	_rotors(_config_index[2]) //XXX + eurocconfig hardcoded
 {
 	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);

From 530c38b6fa3bc0294d66801a0d7a36e8888e903f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 16:11:16 +0100
Subject: [PATCH 238/416] dummy commander and mixer: armed status

---
 .../ros/nodes/commander/commander.cpp         | 24 +++++++++++++------
 src/platforms/ros/nodes/commander/commander.h |  2 ++
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 18 +++++++++++++-
 3 files changed, 36 insertions(+), 8 deletions(-)

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index 1972d8cfa0..d8ff739b9e 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -42,7 +42,6 @@
 
 #include <px4/manual_control_setpoint.h>
 #include <px4/vehicle_control_mode.h>
-#include <px4/actuator_armed.h>
 #include <px4/vehicle_status.h>
 #include <platforms/px4_middleware.h>
 
@@ -53,14 +52,14 @@ Commander::Commander() :
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
-	_msg_parameter_update()
+	_msg_parameter_update(),
+	_msg_actuator_armed()
 {
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg)
 {
 	px4::vehicle_control_mode msg_vehicle_control_mode;
-	px4::actuator_armed msg_actuator_armed;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode */
@@ -71,9 +70,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	msg_vehicle_control_mode.flag_control_attitude_enabled = true;
 
 	/* fill actuator armed */
-	//XXX hardcoded
-	msg_actuator_armed.timestamp = px4::get_time_micros();
-	msg_actuator_armed.armed = true;
+	float arm_th = 0.95;
+	_msg_actuator_armed.timestamp = px4::get_time_micros();
+	if (_msg_actuator_armed.armed) {
+		/* Check for disarm */
+		if (msg->r < -arm_th && msg->z < (1-arm_th)) {
+			_msg_actuator_armed.armed = false;
+		}
+	} else {
+		/* Check for arm */
+		if (msg->r > arm_th && msg->z < (1-arm_th)) {
+			_msg_actuator_armed.armed = true;
+		}
+	}
 
 	/* fill vehicle status */
 	//XXX hardcoded
@@ -83,9 +92,10 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
 	msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
 	msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+	msg_vehicle_status.is_rotary_wing = true;
 
 	_vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
-	_actuator_armed_pub.publish(msg_actuator_armed);
+	_actuator_armed_pub.publish(_msg_actuator_armed);
 	_vehicle_status_pub.publish(msg_vehicle_status);
 
 	/* Fill parameter update */
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index cd9be51351..f3c2f6f1a9 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -41,6 +41,7 @@
 #include "ros/ros.h"
 #include <px4/manual_control_setpoint.h>
 #include <px4/parameter_update.h>
+#include <px4/actuator_armed.h>
 
 class Commander {
 public:
@@ -62,5 +63,6 @@ protected:
 	ros::Publisher _parameter_update_pub;
 
 	px4::parameter_update _msg_parameter_update;
+	px4::actuator_armed _msg_actuator_armed;
 
 };
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index e8c65905f1..4dc428a279 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -54,6 +54,7 @@ public:
 	};
 
 	void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
+	void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg);
 
 private:
 
@@ -73,6 +74,9 @@ private:
 		float control[6];
 	}outputs;
 
+	bool _armed;
+	ros::Subscriber _sub_actuator_armed;
+
 	void mix();
 };
 
@@ -113,6 +117,7 @@ MultirotorMixer::MultirotorMixer():
 	if (!_n.hasParam("motor_scaling_radps")) {
 		_n.setParam("motor_scaling_radps", 1500.0);
 	}
+	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this);
 }
 
 void MultirotorMixer::mix() {
@@ -184,7 +189,13 @@ void MultirotorMixer::mix() {
 	}
 
 	// mix
-	mix();
+	if (_armed) {
+		mix();
+	} else {
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] = 0.0f;
+		}
+	}
 
 	// publish message
 	mav_msgs::MotorSpeed rotor_vel_msg;
@@ -204,3 +215,8 @@ void MultirotorMixer::mix() {
 
   return 0;
 }
+
+void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)
+{
+	_armed = msg.armed;
+}

From 48b781ca3df159cc69358b08f8f5647f2a0d4f3e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 16:11:47 +0100
Subject: [PATCH 239/416] manual input: deadzones

---
 .../ros/nodes/manual_input/manual_input.cpp   | 24 +++++++++++++------
 .../ros/nodes/manual_input/manual_input.h     |  4 +++-
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index eddacf55f1..475d0c4d23 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -48,22 +48,28 @@ ManualInput::ManualInput() :
 	_joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
 	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1))
 {
+	double dz_default = 0.2;
 	/* Load parameters, default values work for Microsoft XBox Controller */
 	_n.param("map_x", _param_axes_map[0], 4);
 	_n.param("scale_x", _param_axes_scale[0], 1.0);
 	_n.param("off_x", _param_axes_offset[0], 0.0);
+	_n.param("dz_x", _param_axes_dz[0], dz_default);
 
 	_n.param("map_y", _param_axes_map[1], 3);
 	_n.param("scale_y", _param_axes_scale[1], -1.0);
 	_n.param("off_y", _param_axes_offset[1], 0.0);
+	_n.param("dz_y", _param_axes_dz[1], dz_default);
 
 	_n.param("map_z", _param_axes_map[2], 2);
 	_n.param("scale_z", _param_axes_scale[2], -0.5);
-	_n.param("off_z", _param_axes_offset[2], 0.5);
+	_n.param("off_z", _param_axes_offset[2], -1.0);
+	_n.param("dz_z", _param_axes_dz[2], dz_default);
 
 	_n.param("map_r", _param_axes_map[3], 0);
 	_n.param("scale_r", _param_axes_scale[3], -1.0);
 	_n.param("off_r", _param_axes_offset[3], 0.0);
+	_n.param("dz_r", _param_axes_dz[3], dz_default);
+
 }
 
 void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
@@ -72,10 +78,10 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 
 	/* Fill px4 manual control setpoint topic with contents from ros joystick */
 	/* Map sticks to x, y, z, r */
-	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], msg_out.x);
-	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], msg_out.y);
-	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], msg_out.z);
-	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], msg_out.r);
+	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x);
+	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y);
+	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z);
+	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r);
 	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
 
 	/* Map buttons to switches */
@@ -93,9 +99,13 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 	_man_ctrl_sp_pub.publish(msg_out);
 }
 
-void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out)
+void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset,
+		double deadzone, float &out)
 {
-	out = (float)(msg->axes[map_index] * scale + offset);
+	double val = msg->axes[map_index];
+	if (val + offset > deadzone || val + offset < -deadzone) {
+		out = (float)((val + offset)) * scale;
+	}
 }
 
 int main(int argc, char **argv)
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index f579acbb77..fb516d3751 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -56,7 +56,8 @@ protected:
 	/**
 	 * Helper function to map and scale joystick input
 	 */
-	void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, float &out);
+	void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, double deadzone,
+			float &out);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _joy_sub;
@@ -66,6 +67,7 @@ protected:
 	int _param_axes_map[4];
 	double _param_axes_scale[4];
 	double _param_axes_offset[4];
+	double _param_axes_dz[4];
 
 
 };

From 06879bff38a88e6468c73dbef5cf6c2ecfdd72b2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 16:12:21 +0100
Subject: [PATCH 240/416] reduce default queue size

---
 src/platforms/px4_nodehandle.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 97fb25563c..0e77852f20 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -140,7 +140,7 @@ public:
 
 
 private:
-	static const uint32_t kQueueSizeDefault = 1000;		/**< Size of queue for ROS */
+	static const uint32_t kQueueSizeDefault = 1;		/**< Size of queue for ROS */
 	std::list<SubscriberBase *> _subs;				/**< Subcriptions of node */
 	std::list<PublisherBase *> _pubs;				/**< Publications of node */
 };

From e16c4ff76e7eff2da88bcbef5d05dd4ba11e7203 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 2 Jan 2015 22:46:50 +0100
Subject: [PATCH 241/416] read attitude and attitude speed from imu message

---
 .../attitude_estimator/attitude_estimator.cpp | 58 +++++++++++++++++--
 .../attitude_estimator/attitude_estimator.h   |  6 +-
 2 files changed, 56 insertions(+), 8 deletions(-)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index 163b111be0..46c836b36b 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -46,7 +46,8 @@
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
-	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
+	// _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
+	_sub_imu(_n.subscribe("/vtol/imu", 1, &AttitudeEstimator::ImuCallback, this)),
 	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
 {
 }
@@ -60,10 +61,11 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	/* Convert quaternion to rotation matrix */
 	math::Quaternion quat;
 	//XXX: search for vtol or other (other than 'plane') vehicle here
-	quat(0) = (float)msg->pose[1].orientation.w;
-	quat(1) = (float)msg->pose[1].orientation.x;
-	quat(2) = (float)-msg->pose[1].orientation.y;
-	quat(3) = (float)-msg->pose[1].orientation.z;
+	int index = 1;
+	quat(0) = (float)msg->pose[index].orientation.w;
+	quat(1) = (float)msg->pose[index].orientation.x;
+	quat(2) = (float)-msg->pose[index].orientation.y;
+	quat(3) = (float)-msg->pose[index].orientation.z;
 
 	msg_v_att.q[0] = quat(0);
 	msg_v_att.q[1] = quat(1);
@@ -83,9 +85,55 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	msg_v_att.pitch = euler(1);
 	msg_v_att.yaw = euler(2);
 
+	//XXX this is in inertial frame
+	// msg_v_att.rollspeed = (float)msg->twist[index].angular.x;
+	// msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y;
+	// msg_v_att.yawspeed = -(float)msg->twist[index].angular.z;
+
 	_vehicle_attitude_pub.publish(msg_v_att);
 }
 
+void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg)
+{
+	px4::vehicle_attitude msg_v_att;
+
+	/* Fill px4 attitude topic with contents from modelstates topic */
+
+	/* Convert quaternion to rotation matrix */
+	math::Quaternion quat;
+	//XXX: search for vtol or other (other than 'plane') vehicle here
+	int index = 1;
+	quat(0) = (float)msg->orientation.w;
+	quat(1) = (float)msg->orientation.x;
+	quat(2) = (float)-msg->orientation.y;
+	quat(3) = (float)-msg->orientation.z;
+
+	msg_v_att.q[0] = quat(0);
+	msg_v_att.q[1] = quat(1);
+	msg_v_att.q[2] = quat(2);
+	msg_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> rot = quat.to_dcm();
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(msg_v_att.R, i, j) = rot(i, j);
+		}
+	}
+	msg_v_att.R_valid = true;
+
+	math::Vector<3> euler = rot.to_euler();
+	msg_v_att.roll = euler(0);
+	msg_v_att.pitch = euler(1);
+	msg_v_att.yaw = euler(2);
+
+	msg_v_att.rollspeed = (float)msg->angular_velocity.x;
+	msg_v_att.pitchspeed = -(float)msg->angular_velocity.y;
+	msg_v_att.yawspeed = -(float)msg->angular_velocity.z;
+
+	_vehicle_attitude_pub.publish(msg_v_att);
+}
+
+
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "attitude_estimator");
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
index e3326b7152..7c09985f34 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
@@ -40,6 +40,7 @@
 
 #include "ros/ros.h"
 #include <gazebo_msgs/ModelStates.h>
+#include <sensor_msgs/Imu.h>
 
 class AttitudeEstimator {
 public:
@@ -48,13 +49,12 @@ public:
 	~AttitudeEstimator() {}
 
 protected:
-	/**
-	 * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
-	 */
 	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg);
+	void ImuCallback(const sensor_msgs::ImuConstPtr& msg);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _sub_modelstates;
+	ros::Subscriber _sub_imu;
 	ros::Publisher _vehicle_attitude_pub;
 
 

From ef485177acb123483fe42d49b3ab23671ee3aa31 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 10:25:39 +0100
Subject: [PATCH 242/416] make Matrix.hpp more consistent with upstream

---
 src/lib/mathlib/math/Matrix.hpp | 29 ++++++++---------------------
 1 file changed, 8 insertions(+), 21 deletions(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index fc57d79647..43ba6b9d9b 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -178,9 +178,8 @@ public:
 	bool operator !=(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j]) {
+				if (data[i][j] != m.data[i][j])
 					return true;
-				}
 
 		return false;
 	}
@@ -202,7 +201,6 @@ public:
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
 				res.data[i][j] = -data[i][j];
-			}
 
 		return res;
 	}
@@ -216,7 +214,6 @@ public:
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
 				res.data[i][j] = data[i][j] + m.data[i][j];
-			}
 
 		return res;
 	}
@@ -225,7 +222,6 @@ public:
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
 				data[i][j] += m.data[i][j];
-			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -237,9 +233,8 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++) {
+			for (unsigned int j = 0; j < N; j++)
 				res.data[i][j] = data[i][j] - m.data[i][j];
-			}
 
 		return res;
 	}
@@ -248,7 +243,6 @@ public:
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
 				data[i][j] -= m.data[i][j];
-			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -260,19 +254,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++) {
+			for (unsigned int j = 0; j < N; j++)
 				res.data[i][j] = data[i][j] * num;
-			}
 
 		return res;
-
 	}
 
 	Matrix<M, N> &operator *=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++) {
+			for (unsigned int j = 0; j < N; j++)
 				data[i][j] *= num;
-			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -281,18 +272,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++) {
+			for (unsigned int j = 0; j < N; j++)
 				res[i][j] = data[i][j] / num;
-			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator /=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++) {
+			for (unsigned int j = 0; j < N; j++)
 				data[i][j] /= num;
-			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -365,18 +354,16 @@ public:
 		memset(data, 0, sizeof(data));
 		unsigned int n = (M < N) ? M : N;
 
-		for (unsigned int i = 0; i < n; i++) {
+		for (unsigned int i = 0; i < n; i++)
 			data[i][i] = 1;
-		}
 	}
 
 	void print(void) {
 		for (unsigned int i = 0; i < M; i++) {
 			printf("[ ");
 
-			for (unsigned int j = 0; j < N; j++) {
+			for (unsigned int j = 0; j < N; j++)
 				printf("%.3f\t", data[i][j]);
-			}
 
 			printf(" ]\n");
 		}

From 17e544ebf3c65338db891edc3615e6fd942f6ab4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 13:09:32 +0100
Subject: [PATCH 243/416] dummy mixer: add offset param

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 25 +++++++++++--------
 1 file changed, 15 insertions(+), 10 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 4dc428a279..5db1800525 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -115,7 +115,10 @@ MultirotorMixer::MultirotorMixer():
 	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);
 	if (!_n.hasParam("motor_scaling_radps")) {
-		_n.setParam("motor_scaling_radps", 1500.0);
+		_n.setParam("motor_scaling_radps", 150.0);
+	}
+	if (!_n.hasParam("motor_offset_radps")) {
+		_n.setParam("motor_offset_radps", 600.0);
 	}
 	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this);
 }
@@ -189,20 +192,22 @@ void MultirotorMixer::mix() {
 	}
 
 	// mix
-	if (_armed) {
-		mix();
-	} else {
-		for (unsigned i = 0; i < _rotor_count; i++) {
-			outputs.control[i] = 0.0f;
-		}
-	}
+	mix();
 
 	// publish message
 	mav_msgs::MotorSpeed rotor_vel_msg;
 	double scaling;
+	double offset;
 	_n.getParamCached("motor_scaling_radps", scaling);
-	for (int i = 0; i < _rotor_count; i++) {
-		rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling);
+	_n.getParamCached("motor_offset_radps", offset);
+	if (_armed) {
+		for (int i = 0; i < _rotor_count; i++) {
+			rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset);
+		}
+	} else {
+		for (int i = 0; i < _rotor_count; i++) {
+			rotor_vel_msg.motor_speed.push_back(0.0);
+		}
 	}
 	_pub.publish(rotor_vel_msg);
 }

From 8d6e1c4455dea2dc13a1a9b08b997ee3df8c7309 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 14:29:10 +0100
Subject: [PATCH 244/416] update ros parameters

---
 launch/multicopter.launch | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 2ccd1338b8..9956c871d9 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -11,7 +11,14 @@
 	<param name="MC_ROLL_P" type="double" value="6.0" />
 	<param name="MC_PITCH_P" type="double" value="6.0" />
 	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
+	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
 	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
+	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
+	<param name="MC_YAW_FF" type="double" value="0.0" />
+	<param name="MC_YAW_P" type="double" value="1.0" />
+	<param name="MC_YAWRATE_P" type="double" value="0.2" />
+	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
+	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
 </group>
 
 </launch>

From 5745189e7b27a7b0b190c746f632318e3c1ee4d7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:22:08 +0100
Subject: [PATCH 245/416] fix some errors/warnings in multiplatform examples

---
 src/examples/publisher/publisher_main.cpp    | 5 +++--
 src/examples/subscriber/subscriber_example.h | 2 ++
 src/examples/subscriber/subscriber_main.cpp  | 5 +++--
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index 5b50ecc6ca..5cac422508 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -52,7 +52,8 @@ using namespace px4;
 PX4_MAIN_FUNCTION(publisher);
 
 #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
-extern "C" __EXPORT int publisher_main(int argc, char *argv[])
+extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
+int publisher_main(int argc, char *argv[])
 {
 	if (argc < 1) {
 		errx(1, "usage: publisher {start|stop|status}");
@@ -73,7 +74,7 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 				       SCHED_PRIORITY_MAX - 5,
 				       2000,
 				       publisher_task_main,
-					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 
 		exit(0);
 	}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index c4b853d4de..eb2c956e09 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -41,6 +41,8 @@
 
 using namespace px4;
 
+void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg);
+
 class SubscriberExample {
 public:
 	SubscriberExample();
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
index 716233739b..c824e34e30 100644
--- a/src/examples/subscriber/subscriber_main.cpp
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -52,7 +52,8 @@ using namespace px4;
 PX4_MAIN_FUNCTION(subscriber);
 
 #if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
-extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
+int subscriber_main(int argc, char *argv[])
 {
 	if (argc < 1) {
 		errx(1, "usage: subscriber {start|stop|status}");
@@ -73,7 +74,7 @@ extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
 				       SCHED_PRIORITY_MAX - 5,
 				       2000,
 				       subscriber_task_main,
-					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 
 		exit(0);
 	}

From 1fe70a845ddd5ba9721748ceced81327efa47193 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:22:43 +0100
Subject: [PATCH 246/416] turn off werror for now

---
 makefiles/toolchain_gnu-arm-eabi.mk | 1 -
 1 file changed, 1 deletion(-)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index ef70d51f20..caaf4ca876 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -127,7 +127,6 @@ ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 #
 ARCHWARNINGS		 = -Wall \
 			   -Wextra \
-			   -Werror \
 			   -Wdouble-promotion \
 			   -Wshadow \
 			   -Wfloat-equal \

From a35818a2884987c0cabddb673638799d420118bd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:23:12 +0100
Subject: [PATCH 247/416] update rc_channels.msg to latest master rc_channels.h

---
 msg/rc_channels.msg | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index 4e0e5b4944..861f3dcaad 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -15,6 +15,9 @@ uint8 RC_CHANNELS_FUNCTION_AUX_2=12
 uint8 RC_CHANNELS_FUNCTION_AUX_3=13
 uint8 RC_CHANNELS_FUNCTION_AUX_4=14
 uint8 RC_CHANNELS_FUNCTION_AUX_5=15
+uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
+uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
+uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint64 timestamp						# Timestamp in microseconds since boot time
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)

From 88a498d8f8598c1ad480df057064d7c2eda2836f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:23:49 +0100
Subject: [PATCH 248/416] sensor: fix variable names

---
 src/modules/sensors/sensors.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1ca9ecd8f3..f27fc127fc 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -834,7 +834,7 @@ Sensors::parameters_update()
 	_rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
 
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		_rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+		_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
 
 	/* gyro offsets */
@@ -891,8 +891,8 @@ Sensors::parameters_update()
 			return ERROR;
  		}
 		close(flowfd);
- 	}	
-	
+ 	}
+
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
 
@@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced)
 		orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
 		/* update paramter handles to which the RC channels are mapped */
 		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-			if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+			if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 				/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 				 * or no request to map this channel to a param has been sent via mavlink
 				 */
@@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc()
 {
 	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+		if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 			 * or no request to map this channel to a param has been sent via mavlink
 			 */
 			continue;
 		}
 
-		float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0);
+		float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
 		/* Check if the value has changed,
 		 * maybe we need to introduce a more aggressive limit here */
 		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {

From 7507e4a4b5dd11bc23fd3ccebc9bcf3aa1162821 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:31:28 +0100
Subject: [PATCH 249/416] sensors: argument is not an enum anymore

---
 src/modules/sensors/sensors.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f27fc127fc..f6ffedc6b5 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1717,7 +1717,7 @@ Sensors::set_params_from_rc()
 			continue;
 		}
 
-		float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
+		float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
 		/* Check if the value has changed,
 		 * maybe we need to introduce a more aggressive limit here */
 		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {

From 9c06450d94a2a462872e5e59ac5285c2d7fe6323 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:31:56 +0100
Subject: [PATCH 250/416] fix RC_CHANNELS_FUNCTION_MAX

---
 msg/rc_channels.msg | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index 861f3dcaad..0fa5ed2fc4 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -1,4 +1,4 @@
-int32 RC_CHANNELS_FUNCTION_MAX=18
+int32 RC_CHANNELS_FUNCTION_MAX=19
 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
 uint8 RC_CHANNELS_FUNCTION_ROLL=1
 uint8 RC_CHANNELS_FUNCTION_PITCH=2
@@ -20,8 +20,8 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint64 timestamp						# Timestamp in microseconds since boot time
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
-float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
+float32[19] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[18] function						# Functions mapping
+int8[19] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout

From 941ff05720b4ba8c282a84e1f8933469ae7fc39e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 15:58:38 +0100
Subject: [PATCH 251/416] uavcan gives compile errors, disable for now

---
 makefiles/config_px4fmu-v2_default.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 86b2f7cf21..e0bd992850 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-MODULES		+= modules/uavcan
+# MODULES		+= modules/uavcan
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

From 5876ff11ec90ca8dee22ee0509ffaec2b561d2fd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 16:12:15 +0100
Subject: [PATCH 252/416] mc att control multiplatform alongside normal mc att
 control

---
 CMakeLists.txt                                |   6 +-
 makefiles/config_px4fmu-v2_default.mk         |   1 +
 .../mc_att_control/mc_att_control_main.cpp    | 954 ++++++++++++++++--
 .../mc_att_control/mc_att_control_params.c    |  42 +-
 src/modules/mc_att_control/module.mk          |   4 +-
 .../mc_att_control.cpp                        |   0
 .../mc_att_control.h                          |   0
 .../mc_att_control_base.cpp                   |   0
 .../mc_att_control_base.h                     |   0
 .../mc_att_control_main.cpp                   | 139 +++
 .../mc_att_control_params.c                   | 248 +++++
 .../mc_att_control_params.h                   |   0
 .../mc_att_control_sim.cpp                    |   0
 .../mc_att_control_sim.h                      |   0
 .../mc_att_control_multiplatform/module.mk    |  43 +
 15 files changed, 1349 insertions(+), 88 deletions(-)
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control.cpp (100%)
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control.h (100%)
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control_base.cpp (100%)
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control_base.h (100%)
 create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
 create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_params.c
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control_params.h (100%)
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control_sim.cpp (100%)
 rename src/modules/{mc_att_control => mc_att_control_multiplatform}/mc_att_control_sim.h (100%)
 create mode 100644 src/modules/mc_att_control_multiplatform/module.mk

diff --git a/CMakeLists.txt b/CMakeLists.txt
index cbd646556a..95a6f12feb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -168,9 +168,9 @@ target_link_libraries(subscriber
 
 ## MC Attitude Control
 add_executable(mc_att_control
-	src/modules/mc_att_control/mc_att_control_main.cpp
-	src/modules/mc_att_control/mc_att_control.cpp
-	src/modules/mc_att_control/mc_att_control_base.cpp)
+	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
 add_dependencies(mc_att_control px4_generate_messages_cpp)
 target_link_libraries(mc_att_control
   ${catkin_LIBRARIES}
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index e0bd992850..d5c3f4a226 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -86,6 +86,7 @@ MODULES		+= modules/position_estimator_inav
 #MODULES		+= modules/fw_pos_control_l1
 #MODULES		+= modules/fw_att_control
 MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_att_control_multiplatform
 MODULES		+= modules/mc_pos_control
 MODULES 	+= modules/vtol_att_control
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 080f0ed65c..67ae90877d 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -38,9 +38,6 @@
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- * @author Julian Oes <julian@oes.ch>
- * @author Roman Bapst <bapstr@ethz.ch>
  *
  * The controller has two loops: P loop for angular error and PD loop for angular rate error.
  * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
@@ -52,91 +49,928 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
-#include <cstdlib>
-#include "mc_att_control.h"
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.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/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
 
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-namespace px4
-{
-bool task_should_exit = false;
-}
-
-using namespace px4;
-
-PX4_MAIN_FUNCTION(mc_att_control);
-void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
-		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
-}
-
-
-#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 /**
  * Multicopter attitude control app start / stop handling function
  *
  * @ingroup apps
  */
+extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 
-extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControl
 {
-	if (argc < 1) {
-		errx(1, "usage: mc_att_control {start|stop|status}");
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControl();
+
+	/**
+	 * Destructor, also kills the main task
+	 */
+	~MulticopterAttitudeControl();
+
+	/**
+	 * Start the multicopter attitude control task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+private:
+
+	bool	_task_should_exit;		/**< if true, task_main() should exit */
+	int		_control_task;			/**< task handle */
+
+	int		_v_att_sub;				/**< vehicle attitude subscription */
+	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
+	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
+	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
+	int		_params_sub;			/**< parameter updates subscription */
+	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
+	int		_armed_sub;				/**< arming status subscription */
+	int		_vehicle_status_sub;	/**< vehicle status subscription */
+
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
+	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+
+	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 */
+
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
+	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
+	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
+	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;			/**< actuator controls */
+	struct actuator_armed_s				_armed;				/**< actuator arming status */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
+	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
+	math::Vector<3>		_rates_int;		/**< angular rates integral error */
+	float				_thrust_sp;		/**< thrust setpoint */
+	math::Vector<3>		_att_control;	/**< attitude control vector */
+
+	math::Matrix<3, 3>  _I;				/**< identity matrix */
+
+	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
+
+	struct {
+		param_t roll_p;
+		param_t roll_rate_p;
+		param_t roll_rate_i;
+		param_t roll_rate_d;
+		param_t pitch_p;
+		param_t pitch_rate_p;
+		param_t pitch_rate_i;
+		param_t pitch_rate_d;
+		param_t yaw_p;
+		param_t yaw_rate_p;
+		param_t yaw_rate_i;
+		param_t yaw_rate_d;
+		param_t yaw_ff;
+		param_t yaw_rate_max;
+
+		param_t man_roll_max;
+		param_t man_pitch_max;
+		param_t man_yaw_max;
+		param_t acro_roll_max;
+		param_t acro_pitch_max;
+		param_t acro_yaw_max;
+
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		math::Vector<3> att_p;					/**< P gain for angular error */
+		math::Vector<3> rate_p;				/**< P gain for angular rate error */
+		math::Vector<3> rate_i;				/**< I gain for angular rate error */
+		math::Vector<3> rate_d;				/**< D gain for angular rate error */
+		float yaw_ff;						/**< yaw control feed-forward */
+		float yaw_rate_max;					/**< max yaw rate */
+
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
+
+	}		_params;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Check for parameter update and handle it.
+	 */
+	void		parameter_update_poll();
+
+	/**
+	 * Check for changes in vehicle control mode.
+	 */
+	void		vehicle_control_mode_poll();
+
+	/**
+	 * Check for changes in manual inputs.
+	 */
+	void		vehicle_manual_poll();
+
+	/**
+	 * Check for attitude setpoint updates.
+	 */
+	void		vehicle_attitude_setpoint_poll();
+
+	/**
+	 * Check for rates setpoint updates.
+	 */
+	void		vehicle_rates_setpoint_poll();
+
+	/**
+	 * Check for arming status updates.
+	 */
+	void		arming_status_poll();
+
+	/**
+	 * Attitude controller.
+	 */
+	void		control_attitude(float dt);
+
+	/**
+	 * Attitude rates controller.
+	 */
+	void		control_attitude_rates(float dt);
+
+	/**
+	 * Check for vehicle status updates.
+	 */
+	void		vehicle_status_poll();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main attitude control task.
+	 */
+	void		task_main();
+};
+
+namespace mc_att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+MulticopterAttitudeControl	*g_control;
+}
+
+MulticopterAttitudeControl::MulticopterAttitudeControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+
+/* subscriptions */
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+	_vehicle_status_sub(-1),
+
+/* publications */
+	_att_sp_pub(-1),
+	_v_rates_sp_pub(-1),
+	_actuators_0_pub(-1),
+	_rates_sp_id(0),
+	_actuators_id(0),
+
+	_actuators_0_circuit_breaker_enabled(false),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
+	memset(&_armed, 0, sizeof(_armed));
+	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
+	_vehicle_status.is_rotary_wing = true;
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
+
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+
+	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
+	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
+	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
+	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
+	_params_handles.yaw_p			=	param_find("MC_YAW_P");
+	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
+	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
+	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
+	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
+	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
+	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
+	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
+	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
+	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+MulticopterAttitudeControl::~MulticopterAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
 	}
 
+	mc_att_control::g_control = nullptr;
+}
+
+int
+MulticopterAttitudeControl::parameters_update()
+{
+	float v;
+
+	/* roll gains */
+	param_get(_params_handles.roll_p, &v);
+	_params.att_p(0) = v;
+	param_get(_params_handles.roll_rate_p, &v);
+	_params.rate_p(0) = v;
+	param_get(_params_handles.roll_rate_i, &v);
+	_params.rate_i(0) = v;
+	param_get(_params_handles.roll_rate_d, &v);
+	_params.rate_d(0) = v;
+
+	/* pitch gains */
+	param_get(_params_handles.pitch_p, &v);
+	_params.att_p(1) = v;
+	param_get(_params_handles.pitch_rate_p, &v);
+	_params.rate_p(1) = v;
+	param_get(_params_handles.pitch_rate_i, &v);
+	_params.rate_i(1) = v;
+	param_get(_params_handles.pitch_rate_d, &v);
+	_params.rate_d(1) = v;
+
+	/* yaw gains */
+	param_get(_params_handles.yaw_p, &v);
+	_params.att_p(2) = v;
+	param_get(_params_handles.yaw_rate_p, &v);
+	_params.rate_p(2) = v;
+	param_get(_params_handles.yaw_rate_i, &v);
+	_params.rate_i(2) = v;
+	param_get(_params_handles.yaw_rate_d, &v);
+	_params.rate_d(2) = v;
+
+	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
+
+	/* manual control scale */
+	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
+	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
+	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);
+
+	/* acro control scale */
+	param_get(_params_handles.acro_roll_max, &v);
+	_params.acro_rate_max(0) = math::radians(v);
+	param_get(_params_handles.acro_pitch_max, &v);
+	_params.acro_rate_max(1) = math::radians(v);
+	param_get(_params_handles.acro_yaw_max, &v);
+	_params.acro_rate_max(2) = math::radians(v);
+
+	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
+
+	return OK;
+}
+
+void
+MulticopterAttitudeControl::parameter_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check if vehicle control mode has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
+	}
+}
+
+void
+MulticopterAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+void
+MulticopterAttitudeControl::vehicle_status_poll()
+{
+	/* check if there is new status information */
+	bool vehicle_status_updated;
+	orb_check(_vehicle_status_sub, &vehicle_status_updated);
+
+	if (vehicle_status_updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
+		}
+	}
+}
+
+/*
+ * Attitude controller.
+ * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
+ * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
+ */
+void
+MulticopterAttitudeControl::control_attitude(float dt)
+{
+	float yaw_sp_move_rate = 0.0f;
+	bool publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.z;
+			publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			/* move yaw setpoint */
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < - yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		R_sp.set(&_v_att_sp.R_body[0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0], &R_sp.data[0], sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+	/* publish the attitude setpoint if needed */
+	if (publish_att_sp && _vehicle_status.is_rotary_wing) {
+		_v_att_sp.timestamp = hrt_absolute_time();
+
+		if (_att_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
+
+		} else {
+			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
+		}
+	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(R.transposed() * R_sp);
+		math::Vector<3> e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+/*
+ * Attitude rates controller.
+ * Input: '_rates_sp' vector, '_thrust_sp'
+ * Output: '_att_control' vector
+ */
+void
+MulticopterAttitudeControl::control_attitude_rates(float dt)
+{
+	/* reset integral if disarmed */
+	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector<3> rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector<3> rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+}
+
+void
+MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	mc_att_control::g_control->task_main();
+}
+
+void
+MulticopterAttitudeControl::task_main()
+{
+
+	/*
+	 * do subscriptions
+	 */
+	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* initialize parameters cache */
+	parameters_update();
+
+	/* wakeup source: vehicle attitude */
+	struct pollfd fds[1];
+
+	fds[0].fd = _v_att_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* run controller on attitude changes */
+		if (fds[0].revents & POLLIN) {
+			static uint64_t last_run = 0;
+			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+			last_run = hrt_absolute_time();
+
+			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+			if (dt < 0.002f) {
+				dt = 0.002f;
+
+			} else if (dt > 0.02f) {
+				dt = 0.02f;
+			}
+
+			/* copy attitude topic */
+			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+			/* check for updates in other topics */
+			parameter_update_poll();
+			vehicle_control_mode_poll();
+			arming_status_poll();
+			vehicle_manual_poll();
+			vehicle_status_poll();
+
+			if (_v_control_mode.flag_control_attitude_enabled) {
+				control_attitude(dt);
+
+				/* publish attitude rates setpoint */
+				_v_rates_sp.roll = _rates_sp(0);
+				_v_rates_sp.pitch = _rates_sp(1);
+				_v_rates_sp.yaw = _rates_sp(2);
+				_v_rates_sp.thrust = _thrust_sp;
+				_v_rates_sp.timestamp = hrt_absolute_time();
+
+				if (_v_rates_sp_pub > 0) {
+					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
+
+				} else if (_rates_sp_id) {
+					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
+				}
+
+			} else {
+				/* attitude controller disabled, poll rates setpoint topic */
+				if (_v_control_mode.flag_control_manual_enabled) {
+					/* manual rates control - ACRO mode */
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+					_thrust_sp = _manual_control_sp.z;
+
+					/* reset yaw setpoint after ACRO */
+					_reset_yaw_sp = true;
+
+					/* publish attitude rates setpoint */
+					_v_rates_sp.roll = _rates_sp(0);
+					_v_rates_sp.pitch = _rates_sp(1);
+					_v_rates_sp.yaw = _rates_sp(2);
+					_v_rates_sp.thrust = _thrust_sp;
+					_v_rates_sp.timestamp = hrt_absolute_time();
+
+					if (_v_rates_sp_pub > 0) {
+						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
+
+					} else if (_rates_sp_id) {
+						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
+					}
+
+				} else {
+					/* attitude controller disabled, poll rates setpoint topic */
+					vehicle_rates_setpoint_poll();
+					_rates_sp(0) = _v_rates_sp.roll;
+					_rates_sp(1) = _v_rates_sp.pitch;
+					_rates_sp(2) = _v_rates_sp.yaw;
+					_thrust_sp = _v_rates_sp.thrust;
+				}
+			}
+
+			if (_v_control_mode.flag_control_rates_enabled) {
+				control_attitude_rates(dt);
+
+				/* publish actuator controls */
+				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+				_actuators.timestamp = hrt_absolute_time();
+
+				if (!_actuators_0_circuit_breaker_enabled) {
+					if (_actuators_0_pub > 0) {
+						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+
+					} else if (_actuators_id) {
+						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
+					}
+
+				}
+			}
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exit");
+
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+MulticopterAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("mc_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int mc_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: mc_att_control {start|stop|status}");
+
 	if (!strcmp(argv[1], "start")) {
 
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
+		if (mc_att_control::g_control != nullptr)
+			errx(1, "already running");
+
+		mc_att_control::g_control = new MulticopterAttitudeControl;
+
+		if (mc_att_control::g_control == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != mc_att_control::g_control->start()) {
+			delete mc_att_control::g_control;
+			mc_att_control::g_control = nullptr;
+			err(1, "start failed");
 		}
 
-		task_should_exit = false;
-
-		daemon_task = task_spawn_cmd("mc_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       3000,
-				       mc_att_control_task_main,
-					(argv) ? (const char **)&argv[2] : (const char **)NULL);
-
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		task_should_exit = true;
+		if (mc_att_control::g_control == nullptr)
+			errx(1, "not running");
+
+		delete mc_att_control::g_control;
+		mc_att_control::g_control = nullptr;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running");
+		if (mc_att_control::g_control) {
+			errx(0, "running");
 
 		} else {
-			warnx("not started");
+			errx(1, "not running");
 		}
-
-		exit(0);
 	}
 
 	warnx("unrecognized command");
 	return 1;
 }
-#endif
-
-PX4_MAIN_FUNCTION(mc_att_control)
-{
-	px4::init(argc, argv, "mc_att_control");
-
-	PX4_INFO("starting");
-	MulticopterAttitudeControl attctl;
-	thread_running = true;
-	attctl.spin();
-
-	PX4_INFO("exiting.");
-	thread_running = false;
-	return 0;
-}
-
-
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index a0b1706f2d..819847b409 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -40,8 +40,6 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <px4_defines.h>
-#include "mc_att_control_params.h"
 #include <systemlib/param/param.h>
 
 /**
@@ -52,7 +50,7 @@
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
+PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
 
 /**
  * Roll rate P gain
@@ -62,7 +60,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
 
 /**
  * Roll rate I gain
@@ -72,7 +70,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
 
 /**
  * Roll rate D gain
@@ -82,7 +80,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
 
 /**
  * Pitch P gain
@@ -93,7 +91,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
+PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
 
 /**
  * Pitch rate P gain
@@ -103,7 +101,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
 
 /**
  * Pitch rate I gain
@@ -113,7 +111,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
 
 /**
  * Pitch rate D gain
@@ -123,7 +121,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
 
 /**
  * Yaw P gain
@@ -134,7 +132,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
+PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
 
 /**
  * Yaw rate P gain
@@ -144,7 +142,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 
 /**
  * Yaw rate I gain
@@ -154,7 +152,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
 
 /**
  * Yaw rate D gain
@@ -164,7 +162,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 
 /**
  * Yaw feed forward
@@ -175,7 +173,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
  * @max 1.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
+PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
 
 /**
  * Max yaw rate
@@ -187,7 +185,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
 
 /**
  * Max manual roll
@@ -197,7 +195,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
+PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
 
 /**
  * Max manual pitch
@@ -207,7 +205,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
+PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
 
 /**
  * Max manual yaw rate
@@ -216,7 +214,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
+PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
 
 /**
  * Max acro roll rate
@@ -226,7 +224,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
 
 /**
  * Max acro pitch rate
@@ -236,7 +234,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
 
 /**
  * Max acro yaw rate
@@ -245,4 +243,4 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk
index 29254f5bb6..64b876f69b 100644
--- a/src/modules/mc_att_control/module.mk
+++ b/src/modules/mc_att_control/module.mk
@@ -38,6 +38,4 @@
 MODULE_COMMAND	= mc_att_control
 
 SRCS		= mc_att_control_main.cpp \
-		  mc_att_control.cpp \
-		  mc_att_control_base.cpp \
-		  mc_att_control_params.c
+			  mc_att_control_params.c
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control.cpp
rename to src/modules/mc_att_control_multiplatform/mc_att_control.cpp
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control.h
rename to src/modules/mc_att_control_multiplatform/mc_att_control.h
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control_base.cpp
rename to src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control_base.h
rename to src/modules/mc_att_control_multiplatform/mc_att_control_base.h
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
new file mode 100644
index 0000000000..69f63a5b11
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_main.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <string.h>
+#include <cstdlib>
+#include "mc_att_control.h"
+
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+
+using namespace px4;
+
+PX4_MAIN_FUNCTION(mc_att_control_multiplatform);
+
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
+/**
+ * Multicopter attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+extern "C" __EXPORT int mc_att_control_multiplatform_main(int argc, char *argv[]);
+int mc_att_control_multiplatform_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_att_control {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       3000,
+				       mc_att_control_multiplatform_task_main,
+					(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
+#endif
+
+PX4_MAIN_FUNCTION(mc_att_control_multiplatform)
+{
+	px4::init(argc, argv, "mc_att_control_multiplatform");
+
+	PX4_INFO("starting");
+	MulticopterAttitudeControl attctl;
+	thread_running = true;
+	attctl.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
+
+
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
new file mode 100644
index 0000000000..a0b1706f2d
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_params.c
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_att_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Roll P gain
+ *
+ * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
+
+/**
+ * Pitch P gain
+ *
+ * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
+
+/**
+ * Yaw P gain
+ *
+ * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
+
+/**
+ * Yaw feed forward
+ *
+ * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
+
+/**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
+
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
+
+/**
+ * Max acro roll rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
+
+/**
+ * Max acro pitch rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
+
+/**
+ * Max acro yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
diff --git a/src/modules/mc_att_control/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control_params.h
rename to src/modules/mc_att_control_multiplatform/mc_att_control_params.h
diff --git a/src/modules/mc_att_control/mc_att_control_sim.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control_sim.cpp
rename to src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
diff --git a/src/modules/mc_att_control/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
similarity index 100%
rename from src/modules/mc_att_control/mc_att_control_sim.h
rename to src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk
new file mode 100644
index 0000000000..7925b83450
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Multirotor attitude controller (vector based, no Euler singularities)
+#
+
+MODULE_COMMAND	= mc_att_control_multiplatform
+
+SRCS		= mc_att_control_main.cpp \
+		  mc_att_control.cpp \
+		  mc_att_control_base.cpp \
+		  mc_att_control_params.c

From 683b06321c192bbc76d322b266c62e414e24f4f4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 16:33:57 +0100
Subject: [PATCH 253/416] Revert "uavcan gives compile errors, disable for now"

This reverts commit 941ff05720b4ba8c282a84e1f8933469ae7fc39e.
---
 makefiles/config_px4fmu-v2_default.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index d5c3f4a226..4bdf7722ca 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-# MODULES		+= modules/uavcan
+MODULES		+= modules/uavcan
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

From 017dc424238e168ba63d540010c580e20b6d9dd7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 16:49:52 +0100
Subject: [PATCH 254/416] multiplatform fix errors from werror

---
 src/platforms/px4_subscriber.h | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index e7e344201f..4b6d5b58f4 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -79,7 +79,8 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	virtual const M& get() = 0;
+	virtual M get() = 0;
+
 	/**
 	 * Get void pointer to last message value
 	 */
@@ -124,7 +125,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	const M& get() { return _msg_current; }
+	M get() { return _msg_current; }
 	/**
 	 * Get void pointer to last message value
 	 */
@@ -204,7 +205,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	const M& get() { return uORB::Subscription<M>::getData(); }
+	M get() { return uORB::Subscription<M>::getData(); }
 	/**
 	 * Get void pointer to last message value
 	 */

From 87650e49e14adae8880dfd06e3d4a23bcb7ae3b1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 5 Jan 2015 16:50:14 +0100
Subject: [PATCH 255/416] Revert "turn off werror for now"

This reverts commit 1fe70a845ddd5ba9721748ceced81327efa47193.
---
 makefiles/toolchain_gnu-arm-eabi.mk | 1 +
 1 file changed, 1 insertion(+)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index caaf4ca876..ef70d51f20 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -127,6 +127,7 @@ ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 #
 ARCHWARNINGS		 = -Wall \
 			   -Wextra \
+			   -Werror \
 			   -Wdouble-promotion \
 			   -Wshadow \
 			   -Wfloat-equal \

From f871b777abe841149ed537349ff7a6bd3156e9c9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 11:09:11 +0100
Subject: [PATCH 256/416] destructors for px4_nodehandle

---
 src/platforms/px4_nodehandle.h | 35 ++++++++++++++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 0e77852f20..3c2bb2d444 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -68,7 +68,8 @@ public:
 
 	~NodeHandle()
 	{
-		//XXX empty lists
+		_subs.clear();
+		_pubs.clear();
 	};
 
 	/**
@@ -154,7 +155,36 @@ public:
 		_sub_min_interval(nullptr)
 	{}
 
-	~NodeHandle() {};
+	~NodeHandle()
+	{
+		/* Empty subscriptions list */
+		uORB::SubscriptionNode *sub = _subs.getHead();
+		int count = 0;
+		while (sub != nullptr) {
+			if (count++ > kMaxSubscriptions) {
+				PX4_WARN("exceeded max subscriptions");
+				break;
+			}
+
+			uORB::SubscriptionNode *sib = sub->getSibling();
+			delete sub;
+			sub = sib;
+		}
+
+		/* Empty publications list */
+		uORB::PublicationNode *pub = _pubs.getHead();
+		count = 0;
+		while (pub != nullptr) {
+			if (count++ > kMaxPublications) {
+				PX4_WARN("exceeded max publications");
+				break;
+			}
+
+			uORB::PublicationNode *sib = pub->getSibling();
+			delete pub;
+			pub = sib;
+		}
+	};
 
 	/**
 	 * Subscribe with callback to function
@@ -254,6 +284,7 @@ public:
 	}
 private:
 	static const uint16_t kMaxSubscriptions = 100;
+	static const uint16_t kMaxPublications = 100;
 	List<uORB::SubscriptionNode *> _subs;		/**< Subcriptions of node */
 	List<uORB::PublicationNode *> _pubs;		/**< Publications of node */
 	uORB::SubscriptionNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval

From 3436abdf0bde16bdda5f4e6600693d6b0f42a01b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 11:10:52 +0100
Subject: [PATCH 257/416] add multiplatform makefile to work around flash size
 issues

---
 makefiles/config_px4fmu-v2_default.mk       |   5 +-
 makefiles/config_px4fmu-v2_multiplatform.mk | 167 ++++++++++++++++++++
 2 files changed, 169 insertions(+), 3 deletions(-)
 create mode 100644 makefiles/config_px4fmu-v2_multiplatform.mk

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 4bdf7722ca..09106c0902 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -83,10 +83,9 @@ MODULES		+= modules/position_estimator_inav
 # Vehicle Control
 #
 #MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
-#MODULES		+= modules/fw_pos_control_l1
-#MODULES		+= modules/fw_att_control
+MODULES		+= modules/fw_pos_control_l1
+MODULES		+= modules/fw_att_control
 MODULES		+= modules/mc_att_control
-MODULES		+= modules/mc_att_control_multiplatform
 MODULES		+= modules/mc_pos_control
 MODULES 	+= modules/vtol_att_control
 
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
new file mode 100644
index 0000000000..7e95ed24e1
--- /dev/null
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -0,0 +1,167 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS, copy the px4iov2 firmware into
+# the ROMFS if it's available
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/px4io
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+# MODULES		+= drivers/sf0x
+MODULES		+= drivers/ll40ls
+# MODULES		+= drivers/trone
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= drivers/hott
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
+# MODULES		+= drivers/blinkm
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/frsky_telemetry
+MODULES		+= modules/sensors
+MODULES		+= drivers/mkblctrl
+MODULES		+= drivers/px4flow
+
+#
+# System commands
+#
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/dumpfile
+MODULES		+= systemcmds/ver
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+MODULES		+= modules/gpio_led
+MODULES		+= modules/uavcan
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/ekf_att_pos_estimator
+MODULES		+= modules/position_estimator_inav
+
+#
+# Vehicle Control
+#
+#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
+#MODULES		+= modules/fw_pos_control_l1
+#MODULES		+= modules/fw_att_control
+# MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_att_control_multiplatform
+MODULES		+= modules/mc_pos_control
+MODULES 	+= modules/vtol_att_control
+
+#
+# Logging
+#
+MODULES		+= modules/sdlog2
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+MODULES		+= modules/dataman
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/geo_lookup
+MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
+
+#
+# OBC challenge
+#
+MODULES		+= modules/bottle_drop
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+# Generate parameter XML file
+GEN_PARAM_XML = 1
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )

From 581d4e111082dbe8cde4a7ae3115f30c7a7417fe Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 11:58:04 +0100
Subject: [PATCH 258/416] travis: apt-get install python-empy

---
 .travis.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index fd2a6b6d1c..9386ddaa31 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -12,7 +12,7 @@ before_script:
   - sudo apt-get install s3cmd grep zip mailutils
 # General toolchain dependencies
   - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386
-  - sudo apt-get install python-serial python-argparse
+  - sudo apt-get install python-serial python-argparse python-empy
   - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake
   - pushd .
   - cd ~

From 0893f9fd96200c58bec5ed9f2b0965184c4ae45e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 19:13:39 +0100
Subject: [PATCH 259/416] remove uavcan from multiplatform build, problems with
 linking

---
 makefiles/config_px4fmu-v2_multiplatform.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index 7e95ed24e1..bed2fa5061 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-MODULES		+= modules/uavcan
+# MODULES		+= modules/uavcan
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

From 5056b03ab00a11c3705a97edb9bbfc13a32130dd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 19:14:01 +0100
Subject: [PATCH 260/416] geo: fix include

---
 src/lib/geo/geo.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 0127796460..ca3587b934 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -44,7 +44,7 @@
  */
 
 #pragma once
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 __BEGIN_DECLS
 
 #include "geo_lookup/geo_mag_declination.h"

From ee561947e9100f64a206216104e3ce83e8a304ca Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 19:14:57 +0100
Subject: [PATCH 261/416] makefile: fix order

---
 Makefile | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index b800459cb4..37ba35feda 100644
--- a/Makefile
+++ b/Makefile
@@ -104,7 +104,10 @@ DESIRED_FIRMWARES 	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
 STAGED_FIRMWARES	 = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
 FIRMWARES		 = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
 
-all:	checksubmodules generateuorbtopicheaders $(DESIRED_FIRMWARES)
+all:
+	$(Q) $(MAKE) checksubmodules
+	$(Q) $(MAKE) generateuorbtopicheaders
+	$(Q) $(MAKE) $(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.

From f37fdd95af06d7b6937cb53d34ea8777342b3aef Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 19:45:57 +0100
Subject: [PATCH 262/416] add and use PX4_ROS preprocessor define

---
 CMakeLists.txt                                            | 1 +
 src/examples/publisher/publisher_main.cpp                 | 2 +-
 src/examples/subscriber/subscriber_main.cpp               | 2 +-
 src/lib/mathlib/math/Matrix.hpp                           | 2 +-
 .../mc_att_control_multiplatform/mc_att_control_main.cpp  | 2 +-
 src/platforms/px4_defines.h                               | 2 +-
 src/platforms/px4_includes.h                              | 2 +-
 src/platforms/px4_middleware.h                            | 4 ++--
 src/platforms/px4_nodehandle.h                            | 4 ++--
 src/platforms/px4_publisher.h                             | 4 ++--
 src/platforms/px4_subscriber.h                            | 8 +++++---
 11 files changed, 18 insertions(+), 15 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95a6f12feb..eca28b15e6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(px4)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+add_definitions(-D__PX4_ROS)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index 5cac422508..81439a8be2 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -51,7 +51,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(publisher);
 
-#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
+#if !defined(__PX4_ROS)
 extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
 int publisher_main(int argc, char *argv[])
 {
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
index c824e34e30..906921e019 100644
--- a/src/examples/subscriber/subscriber_main.cpp
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -51,7 +51,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(subscriber);
 
-#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
+#if !defined(__PX4_ROS)
 extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
 int subscriber_main(int argc, char *argv[])
 {
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 43ba6b9d9b..68f0a777db 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -122,7 +122,7 @@ public:
 		memcpy(data, d, sizeof(data));
 	}
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 	/**
 	 * set data from boost::array
 	 */
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
index 69f63a5b11..e2f2f2530c 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -67,7 +67,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(mc_att_control_multiplatform);
 
-#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
+#if !defined(__PX4_ROS)
 /**
  * Multicopter attitude control app start / stop handling function
  *
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index ef43c55b83..457b5abe37 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -46,7 +46,7 @@
 #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
 
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 /*
  * Building for running within the ROS environment
  */
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 4d29bc3cd8..fc31162c8a 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -41,7 +41,7 @@
 
 #include <stdbool.h>
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 /*
  * Building for running within the ROS environment
  */
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index 54050de8b3..cd52fd650b 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -42,7 +42,7 @@
 #include <stdint.h>
 #include <unistd.h>
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 #define __EXPORT
 #endif
 
@@ -53,7 +53,7 @@ __EXPORT void init(int argc, char *argv[], const char *process_name);
 
 __EXPORT uint64_t get_time_micros();
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 /**
  * Returns true if the app/task should continue to run
  */
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 3c2bb2d444..624a466fdf 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -43,7 +43,7 @@
 #include "px4_publisher.h"
 #include "px4_middleware.h"
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 /* includes when building for ros */
 #include "ros/ros.h"
 #include <list>
@@ -55,7 +55,7 @@
 
 namespace px4
 {
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 class NodeHandle :
 	private ros::NodeHandle
 {
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 6b6d8e39e9..c6f3d6108e 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -37,7 +37,7 @@
  * PX4 Middleware Wrapper Node Handle
  */
 #pragma once
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 /* includes when building for ros */
 #include "ros/ros.h"
 #else
@@ -60,7 +60,7 @@ public:
 
 };
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 class Publisher :
 	public PublisherBase
 {
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 4b6d5b58f4..107c60189e 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -36,9 +36,11 @@
  *
  * PX4 Middleware Wrapper Subscriber
  */
-#include <functional>
 #pragma once
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+
+#include <functional>
+
+#if defined(__PX4_ROS)
 /* includes when building for ros */
 #include "ros/ros.h"
 #else
@@ -87,7 +89,7 @@ public:
 	virtual void * get_void_ptr() = 0;
 };
 
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#if defined(__PX4_ROS)
 /**
  * Subscriber class that is templated with the ros n message type
  */

From 213f08ee07c57d4781002cefe797f7ce9e9fd561 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 Jan 2015 23:52:04 +0100
Subject: [PATCH 263/416] generate uorb topic headers for tests make target

---
 Makefile | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Makefile b/Makefile
index 37ba35feda..859eca0cea 100644
--- a/Makefile
+++ b/Makefile
@@ -255,6 +255,7 @@ testbuild:
 # unit tests.
 .PHONY: tests
 tests:
+	$(Q) $(MAKE) generateuorbtopicheaders
 	$(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests)
 
 #

From e855e4454cce0a3bdc51b785a44d7660ae5a921d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 7 Jan 2015 00:19:30 +0100
Subject: [PATCH 264/416] exclude macro hack for tests target

---
 src/platforms/px4_defines.h | 2 ++
 unittests/CMakeLists.txt    | 1 +
 2 files changed, 3 insertions(+)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 457b5abe37..28eef7e187 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -177,9 +177,11 @@ typedef param_t px4_param_t;
 #define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt)
 
 /* XXX this is a hack to resolve conflicts with NuttX headers */
+#if !defined(__PX4_TESTS)
 #define isspace(c) \
   ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
    (c) == '\r' || (c) == '\f' || c== '\v')
+#endif
 
 #endif
 
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index ec3e4104ec..0737f66962 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -11,6 +11,7 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/modules)
 include_directories(${CMAKE_SOURCE_DIR}/../src/lib)
 
 add_definitions(-D__EXPORT=)
+add_definitions(-D__PX4_TESTS)
 
 function(add_gtest)
   foreach(test_name ${ARGN})

From a01a8f955336e2825d23d5494703ea91f15e3ee1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 7 Jan 2015 08:20:35 +0100
Subject: [PATCH 265/416] add missing defines for unittests

---
 unittests/CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 0737f66962..47b84f1480 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -12,6 +12,8 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/lib)
 
 add_definitions(-D__EXPORT=)
 add_definitions(-D__PX4_TESTS)
+add_definitions(-Dnoreturn_function=)
+add_definitions(-Dmain_t=int)
 
 function(add_gtest)
   foreach(test_name ${ARGN})

From 417a82c699c2512ca3b8998c91c9d77f5d826edb Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 7 Jan 2015 13:48:20 +0100
Subject: [PATCH 266/416] add ros setup scripts

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 31 ++++++++++++++++++++++++
 Tools/ros/px4_workspace_create.sh        |  7 ++++++
 Tools/ros/px4_workspace_setup.sh         | 23 ++++++++++++++++++
 3 files changed, 61 insertions(+)
 create mode 100755 Tools/ros/px4_ros_installation_ubuntu.sh
 create mode 100755 Tools/ros/px4_workspace_create.sh
 create mode 100755 Tools/ros/px4_workspace_setup.sh

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
new file mode 100755
index 0000000000..c6f27653cf
--- /dev/null
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -0,0 +1,31 @@
+#!/bin/sh
+
+# main ROS Setup
+# following http://wiki.ros.org/indigo/Installation/Ubuntu
+# run this file with . ./px4_ros_setup_ubuntu.sh
+
+## add ROS repository
+sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+
+## add key
+wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \
+    sudo apt-key add -
+
+## Install main ROS pacakges
+sudo apt-get update
+sudo apt-get install ros-indigo-desktop-full
+sudo rosdep init
+rosdep update
+
+## Setup environment variables
+echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
+source ~/.bashrc
+
+# get rosinstall
+sudo apt-get install python-rosinstall
+
+# Hector packages
+sudo apt-get install ros-indigo-hector-quadrotor \
+    ros-indigo-octomap-msgs \
+    ros-indigo-hector-gazebo
+
diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh
new file mode 100755
index 0000000000..d8d65c8199
--- /dev/null
+++ b/Tools/ros/px4_workspace_create.sh
@@ -0,0 +1,7 @@
+#!/bin/sh
+# this script creates a catkin_ws in the current folder
+
+mkdir -p catkin_ws/src
+cd catkin_ws/src
+catkin_init_workspace
+cd ..
diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
new file mode 100755
index 0000000000..233d216718
--- /dev/null
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -0,0 +1,23 @@
+#!/bin/sh
+# run this script from the root of your catkin_ws
+source devel/setup.bash
+cd src
+
+# PX4 Firmware
+git clone https://github.com/PX4/Firmware.git
+cd Firmware
+git checkout ros
+cd ..
+
+# euroc simulator
+git clone https://github.com/PX4/euroc_simulator.git
+cd euroc_simulator
+git checkout px4_nodes
+cd ..
+
+# # mav comm
+git clone https://github.com/PX4/mav_comm.git
+
+cd ..
+
+catkin_make

From 154111d4c011a9fd58812cb88fbbbbc38e30e08d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 7 Jan 2015 17:01:03 +0100
Subject: [PATCH 267/416] move checksubmodules and generateuorbtopicheaders
 dependency

---
 Makefile | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/Makefile b/Makefile
index 2ba2073eb4..6adbf765ee 100644
--- a/Makefile
+++ b/Makefile
@@ -104,10 +104,7 @@ DESIRED_FIRMWARES 	 = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
 STAGED_FIRMWARES	 = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
 FIRMWARES		 = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
 
-all:
-	$(Q) $(MAKE) checksubmodules
-	$(Q) $(MAKE) generateuorbtopicheaders
-	$(Q) $(MAKE) $(DESIRED_FIRMWARES)
+all:	$(DESIRED_FIRMWARES)
 
 #
 # Copy FIRMWARES into the image directory.
@@ -127,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
 .PHONY: $(FIRMWARES)
 $(BUILD_DIR)%.build/firmware.px4: config   = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
 $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
-$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
+$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	checksubmodules generateuorbtopicheaders
 	@$(ECHO) %%%%
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%

From 143ff444e46a20709b1cd50cb732b3c5640d8ecd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 7 Jan 2015 17:28:56 +0100
Subject: [PATCH 268/416] fix merge error of tests target change

---
 Makefile | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/Makefile b/Makefile
index 6adbf765ee..6480286d2d 100644
--- a/Makefile
+++ b/Makefile
@@ -251,9 +251,7 @@ testbuild:
 # Unittest targets. Builds and runs the host-level
 # unit tests.
 .PHONY: tests
-tests:
-	$(Q) $(MAKE) generateuorbtopicheaders
-	$(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests)
+tests:	generateuorbtopicheaders
 	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
 
 #

From 34b023d0a62bef93feb0ce3131e677f7c8546b45 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 08:08:45 +0100
Subject: [PATCH 269/416] revert strange change in nsh Make.defs

---
 nuttx-configs/px4fmu-v2/nsh/Make.defs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
index e0e868870a..99f3b3140d 100644
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -90,7 +90,7 @@ else
     ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
     ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
   else
-    # Linux/Cygwin-native toolchain
+    # Linux/Cygwin-native toolchain 
     MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
     ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
     ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
@@ -117,7 +117,7 @@ ARCHOPTIMIZATION	+= -g
 endif
 
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=c++11
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
 ARCHWARNINGS		 = -Wall \
 			   -Wno-sign-compare \
 			   -Wextra \

From c118d17cb576b76df3bc1b765fb38c34421c7be4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 08:15:44 +0100
Subject: [PATCH 270/416] fix code style in src/platforms

---
 src/platforms/px4_defines.h                   |  6 ++-
 src/platforms/px4_nodehandle.h                | 27 ++++++----
 src/platforms/px4_subscriber.h                | 24 +++++----
 src/platforms/ros/eigen_math.h                |  3 +-
 src/platforms/ros/geo.cpp                     |  8 +++
 .../attitude_estimator/attitude_estimator.cpp | 24 +++++----
 .../attitude_estimator/attitude_estimator.h   |  7 +--
 .../ros/nodes/commander/commander.cpp         | 16 +++---
 src/platforms/ros/nodes/commander/commander.h |  5 +-
 .../ros/nodes/manual_input/manual_input.cpp   | 15 +++---
 .../ros/nodes/manual_input/manual_input.h     |  9 ++--
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 53 +++++++++++--------
 12 files changed, 117 insertions(+), 80 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 28eef7e187..6b6a03893b 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -84,6 +84,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
 	if (!ros::param::has(name)) {
 		ros::param::set(name, value);
 	}
+
 	return (px4_param_t)name;
 };
 static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
@@ -91,6 +92,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 	if (!ros::param::has(name)) {
 		ros::param::set(name, value);
 	}
+
 	return (px4_param_t)name;
 };
 
@@ -179,8 +181,8 @@ typedef param_t px4_param_t;
 /* XXX this is a hack to resolve conflicts with NuttX headers */
 #if !defined(__PX4_TESTS)
 #define isspace(c) \
-  ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
-   (c) == '\r' || (c) == '\f' || c== '\v')
+	((c) == ' '  || (c) == '\t' || (c) == '\n' || \
+	 (c) == '\r' || (c) == '\f' || c== '\v')
 #endif
 
 #endif
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 624a466fdf..7b14caed97 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -78,11 +78,12 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M>
-	Subscriber<M> *subscribe(const char *topic, void(*fp)(const M&))
+	Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
 	{
 		SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
-		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+					  (SubscriberROS<M> *)sub);
+		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
 		return (Subscriber<M> *)sub;
 	}
@@ -93,11 +94,12 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M, typename T>
-	Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M&), T *obj)
+	Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
 	{
 		SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
-		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+					  (SubscriberROS<M> *)sub);
+		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
 		return (Subscriber<M> *)sub;
 	}
@@ -110,8 +112,9 @@ public:
 	Subscriber<M> *subscribe(const char *topic)
 	{
 		SubscriberBase *sub = new SubscriberROS<M>();
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub);
-		((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub);
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+					  (SubscriberROS<M> *)sub);
+		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
 		return (Subscriber<M> *)sub;
 	}
@@ -160,6 +163,7 @@ public:
 		/* Empty subscriptions list */
 		uORB::SubscriptionNode *sub = _subs.getHead();
 		int count = 0;
+
 		while (sub != nullptr) {
 			if (count++ > kMaxSubscriptions) {
 				PX4_WARN("exceeded max subscriptions");
@@ -174,6 +178,7 @@ public:
 		/* Empty publications list */
 		uORB::PublicationNode *pub = _pubs.getHead();
 		count = 0;
+
 		while (pub != nullptr) {
 			if (count++ > kMaxPublications) {
 				PX4_WARN("exceeded max publications");
@@ -195,8 +200,8 @@ public:
 
 	template<typename M>
 	Subscriber<M> *subscribe(const struct orb_metadata *meta,
-			      std::function<void(const M &)> callback,
-			      unsigned interval)
+				 std::function<void(const M &)> callback,
+				 unsigned interval)
 	{
 		SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
 
@@ -216,7 +221,7 @@ public:
 
 	template<typename M>
 	Subscriber<M> *subscribe(const struct orb_metadata *meta,
-			      unsigned interval)
+				 unsigned interval)
 	{
 		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
 
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 107c60189e..aaacf9e484 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -86,7 +86,7 @@ public:
 	/**
 	 * Get void pointer to last message value
 	 */
-	virtual void * get_void_ptr() = 0;
+	virtual void *get_void_ptr() = 0;
 };
 
 #if defined(__PX4_ROS)
@@ -97,7 +97,7 @@ template<typename M>
 class SubscriberROS :
 	public Subscriber<M>
 {
-friend class NodeHandle;
+	friend class NodeHandle;
 
 public:
 	/**
@@ -131,13 +131,14 @@ public:
 	/**
 	 * Get void pointer to last message value
 	 */
-	void * get_void_ptr() { return (void*)&_msg_current; }
+	void *get_void_ptr() { return (void *)&_msg_current; }
 
 protected:
 	/**
 	 * Called on topic update, saves the current message and then calls the provided callback function
 	 */
-	void callback(const M &msg) {
+	void callback(const M &msg)
+	{
 		/* Store data */
 		_msg_current = msg;
 
@@ -151,7 +152,8 @@ protected:
 	/**
 	 * Saves the ros subscriber to keep ros subscription alive
 	 */
-	void set_ros_sub(ros::Subscriber ros_sub) {
+	void set_ros_sub(ros::Subscriber ros_sub)
+	{
 		_ros_sub = ros_sub;
 	}
 
@@ -180,8 +182,8 @@ public:
 	 * @param list	    subscriber is added to this list
 	 */
 	SubscriberUORB(const struct orb_metadata *meta,
-		      unsigned interval,
-		      List<uORB::SubscriptionNode *> *list) :
+		       unsigned interval,
+		       List<uORB::SubscriptionNode *> *list) :
 		Subscriber<M>(),
 		uORB::Subscription<M>(meta, interval, list)
 	{}
@@ -211,7 +213,7 @@ public:
 	/**
 	 * Get void pointer to last message value
 	 */
-	void * get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); }
+	void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); }
 };
 
 template<typename M>
@@ -227,9 +229,9 @@ public:
 	 * @param list	    subscriber is added to this list
 	 */
 	SubscriberUORBCallback(const struct orb_metadata *meta,
-		      unsigned interval,
-		      std::function<void(const M &)> callback,
-		      List<uORB::SubscriptionNode *> *list) :
+			       unsigned interval,
+			       std::function<void(const M &)> callback,
+			       List<uORB::SubscriptionNode *> *list) :
 		SubscriberUORB<M>(meta, interval, list),
 		_callback(callback)
 	{}
diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h
index 60fc9b93cd..c7271c1575 100755
--- a/src/platforms/ros/eigen_math.h
+++ b/src/platforms/ros/eigen_math.h
@@ -9,8 +9,7 @@
 #define EIGEN_MATH_H_
 
 
-struct eigen_matrix_instance
-{
+struct eigen_matrix_instance {
 	int numRows;
 	int numCols;
 	float *pData;
diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
index a7ee6fd844..6fad681c99 100644
--- a/src/platforms/ros/geo.cpp
+++ b/src/platforms/ros/geo.cpp
@@ -60,6 +60,7 @@ __EXPORT float _wrap_pi(float bearing)
 	}
 
 	int c = 0;
+
 	while (bearing >= M_PI_F) {
 		bearing -= M_TWOPI_F;
 
@@ -69,6 +70,7 @@ __EXPORT float _wrap_pi(float bearing)
 	}
 
 	c = 0;
+
 	while (bearing < -M_PI_F) {
 		bearing += M_TWOPI_F;
 
@@ -88,6 +90,7 @@ __EXPORT float _wrap_2pi(float bearing)
 	}
 
 	int c = 0;
+
 	while (bearing >= M_TWOPI_F) {
 		bearing -= M_TWOPI_F;
 
@@ -97,6 +100,7 @@ __EXPORT float _wrap_2pi(float bearing)
 	}
 
 	c = 0;
+
 	while (bearing < 0.0f) {
 		bearing += M_TWOPI_F;
 
@@ -116,6 +120,7 @@ __EXPORT float _wrap_180(float bearing)
 	}
 
 	int c = 0;
+
 	while (bearing >= 180.0f) {
 		bearing -= 360.0f;
 
@@ -125,6 +130,7 @@ __EXPORT float _wrap_180(float bearing)
 	}
 
 	c = 0;
+
 	while (bearing < -180.0f) {
 		bearing += 360.0f;
 
@@ -144,6 +150,7 @@ __EXPORT float _wrap_360(float bearing)
 	}
 
 	int c = 0;
+
 	while (bearing >= 360.0f) {
 		bearing -= 360.0f;
 
@@ -153,6 +160,7 @@ __EXPORT float _wrap_360(float bearing)
 	}
 
 	c = 0;
+
 	while (bearing < 0.0f) {
 		bearing += 360.0f;
 
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index 46c836b36b..ce863d10e3 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -52,7 +52,7 @@ AttitudeEstimator::AttitudeEstimator() :
 {
 }
 
-void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg)
+void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
 {
 	px4::vehicle_attitude msg_v_att;
 
@@ -64,8 +64,8 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	int index = 1;
 	quat(0) = (float)msg->pose[index].orientation.w;
 	quat(1) = (float)msg->pose[index].orientation.x;
-	quat(2) = (float)-msg->pose[index].orientation.y;
-	quat(3) = (float)-msg->pose[index].orientation.z;
+	quat(2) = (float) - msg->pose[index].orientation.y;
+	quat(3) = (float) - msg->pose[index].orientation.z;
 
 	msg_v_att.q[0] = quat(0);
 	msg_v_att.q[1] = quat(1);
@@ -73,11 +73,13 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	msg_v_att.q[3] = quat(3);
 
 	math::Matrix<3, 3> rot = quat.to_dcm();
+
 	for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
 			PX4_R(msg_v_att.R, i, j) = rot(i, j);
 		}
 	}
+
 	msg_v_att.R_valid = true;
 
 	math::Vector<3> euler = rot.to_euler();
@@ -93,7 +95,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	_vehicle_attitude_pub.publish(msg_v_att);
 }
 
-void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg)
+void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
 {
 	px4::vehicle_attitude msg_v_att;
 
@@ -105,8 +107,8 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg)
 	int index = 1;
 	quat(0) = (float)msg->orientation.w;
 	quat(1) = (float)msg->orientation.x;
-	quat(2) = (float)-msg->orientation.y;
-	quat(3) = (float)-msg->orientation.z;
+	quat(2) = (float) - msg->orientation.y;
+	quat(3) = (float) - msg->orientation.z;
 
 	msg_v_att.q[0] = quat(0);
 	msg_v_att.q[1] = quat(1);
@@ -114,11 +116,13 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg)
 	msg_v_att.q[3] = quat(3);
 
 	math::Matrix<3, 3> rot = quat.to_dcm();
+
 	for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
 			PX4_R(msg_v_att.R, i, j) = rot(i, j);
 		}
 	}
+
 	msg_v_att.R_valid = true;
 
 	math::Vector<3> euler = rot.to_euler();
@@ -136,10 +140,10 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg)
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "attitude_estimator");
-  AttitudeEstimator m;
+	ros::init(argc, argv, "attitude_estimator");
+	AttitudeEstimator m;
 
-  ros::spin();
+	ros::spin();
 
-  return 0;
+	return 0;
 }
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
index 7c09985f34..f760a39d8b 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
@@ -42,15 +42,16 @@
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/Imu.h>
 
-class AttitudeEstimator {
+class AttitudeEstimator
+{
 public:
 	AttitudeEstimator();
 
 	~AttitudeEstimator() {}
 
 protected:
-	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg);
-	void ImuCallback(const sensor_msgs::ImuConstPtr& msg);
+	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
+	void ImuCallback(const sensor_msgs::ImuConstPtr &msg);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _sub_modelstates;
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index d8ff739b9e..b9fc296f9d 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -57,7 +57,7 @@ Commander::Commander() :
 {
 }
 
-void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg)
+void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
 	px4::vehicle_control_mode msg_vehicle_control_mode;
 	px4::vehicle_status msg_vehicle_status;
@@ -72,14 +72,16 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	/* fill actuator armed */
 	float arm_th = 0.95;
 	_msg_actuator_armed.timestamp = px4::get_time_micros();
+
 	if (_msg_actuator_armed.armed) {
 		/* Check for disarm */
-		if (msg->r < -arm_th && msg->z < (1-arm_th)) {
+		if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
 			_msg_actuator_armed.armed = false;
 		}
+
 	} else {
 		/* Check for arm */
-		if (msg->r > arm_th && msg->z < (1-arm_th)) {
+		if (msg->r > arm_th && msg->z < (1 - arm_th)) {
 			_msg_actuator_armed.armed = true;
 		}
 	}
@@ -107,8 +109,8 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "commander");
-  Commander m;
-  ros::spin();
-  return 0;
+	ros::init(argc, argv, "commander");
+	Commander m;
+	ros::spin();
+	return 0;
 }
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index f3c2f6f1a9..bd4092b3a2 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -43,7 +43,8 @@
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
 
-class Commander {
+class Commander
+{
 public:
 	Commander();
 
@@ -53,7 +54,7 @@ protected:
 	/**
 	 * Based on manual control input the status will be set
 	 */
-	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg);
+	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 475d0c4d23..688df50e0c 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -72,7 +72,7 @@ ManualInput::ManualInput() :
 
 }
 
-void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
+void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
 {
 	px4::manual_control_setpoint msg_out;
 
@@ -99,10 +99,11 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg)
 	_man_ctrl_sp_pub.publish(msg_out);
 }
 
-void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset,
-		double deadzone, float &out)
+void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset,
+			  double deadzone, float &out)
 {
 	double val = msg->axes[map_index];
+
 	if (val + offset > deadzone || val + offset < -deadzone) {
 		out = (float)((val + offset)) * scale;
 	}
@@ -110,8 +111,8 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, do
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "manual_input");
-  ManualInput m;
-  ros::spin();
-  return 0;
+	ros::init(argc, argv, "manual_input");
+	ManualInput m;
+	ros::spin();
+	return 0;
 }
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index fb516d3751..93e0abe64d 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -41,7 +41,8 @@
 #include "ros/ros.h"
 #include <sensor_msgs/Joy.h>
 
-class ManualInput {
+class ManualInput
+{
 public:
 	ManualInput();
 
@@ -51,13 +52,13 @@ protected:
 	/**
 	 * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
 	 */
-	void JoyCallback(const sensor_msgs::JoyConstPtr& msg);
+	void JoyCallback(const sensor_msgs::JoyConstPtr &msg);
 
 	/**
 	 * Helper function to map and scale joystick input
 	 */
-	void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, double deadzone,
-			float &out);
+	void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
+		     float &out);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _joy_sub;
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 5db1800525..e2180af41b 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -37,12 +37,13 @@
  *
  * @author Roman Bapst <romanbapst@yahoo.de>
 */
- #include <ros/ros.h>
- #include <px4.h>
- #include <lib/mathlib/mathlib.h>
- #include <mav_msgs/MotorSpeed.h>
+#include <ros/ros.h>
+#include <px4.h>
+#include <lib/mathlib/mathlib.h>
+#include <mav_msgs/MotorSpeed.h>
 
- class MultirotorMixer {
+class MultirotorMixer
+{
 public:
 
 	MultirotorMixer();
@@ -68,11 +69,11 @@ private:
 
 	struct {
 		float control[6];
-	}inputs;
+	} inputs;
 
 	struct  {
 		float control[6];
-	}outputs;
+	} outputs;
 
 	bool _armed;
 	ros::Subscriber _sub_actuator_armed;
@@ -112,18 +113,22 @@ MultirotorMixer::MultirotorMixer():
 	_rotor_count(4),
 	_rotors(_config_index[2]) //XXX + eurocconfig hardcoded
 {
-	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this);
-	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10);
+	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
+	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);
+
 	if (!_n.hasParam("motor_scaling_radps")) {
 		_n.setParam("motor_scaling_radps", 150.0);
 	}
+
 	if (!_n.hasParam("motor_offset_radps")) {
 		_n.setParam("motor_offset_radps", 600.0);
 	}
-	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this);
+
+	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this);
 }
 
-void MultirotorMixer::mix() {
+void MultirotorMixer::mix()
+{
 	float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
 	float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
 	float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f);
@@ -134,7 +139,7 @@ void MultirotorMixer::mix() {
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float out = roll * _rotors[i].roll_scale
-				+ pitch * _rotors[i].pitch_scale + thrust;
+			    + pitch * _rotors[i].pitch_scale + thrust;
 
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
@@ -145,12 +150,14 @@ void MultirotorMixer::mix() {
 		if (out < min_out) {
 			min_out = out;
 		}
+
 		if (out > max_out) {
 			max_out = out;
 		}
 
 		outputs.control[i] = out;
 	}
+
 	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
 	if (min_out < 0.0f) {
 		float scale_in = thrust / (thrust - min_out);
@@ -158,8 +165,8 @@ void MultirotorMixer::mix() {
 		/* mix again with adjusted controls */
 		for (unsigned i = 0; i < _rotor_count; i++) {
 			outputs.control[i] = scale_in
-					* (roll * _rotors[i].roll_scale
-							+ pitch * _rotors[i].pitch_scale) + thrust;
+					     * (roll * _rotors[i].roll_scale
+						+ pitch * _rotors[i].pitch_scale) + thrust;
 		}
 
 	} else {
@@ -171,6 +178,7 @@ void MultirotorMixer::mix() {
 
 	/* scale down all outputs if some outputs are too large, reduce total thrust */
 	float scale_out;
+
 	if (max_out > 1.0f) {
 		scale_out = 1.0f / max_out;
 
@@ -184,10 +192,10 @@ void MultirotorMixer::mix() {
 	}
 }
 
- void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
+void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
 {
 	// read message
-	for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) {
+	for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) {
 		inputs.control[i] = msg.control[i];
 	}
 
@@ -200,25 +208,28 @@ void MultirotorMixer::mix() {
 	double offset;
 	_n.getParamCached("motor_scaling_radps", scaling);
 	_n.getParamCached("motor_offset_radps", offset);
+
 	if (_armed) {
 		for (int i = 0; i < _rotor_count; i++) {
 			rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset);
 		}
+
 	} else {
 		for (int i = 0; i < _rotor_count; i++) {
 			rotor_vel_msg.motor_speed.push_back(0.0);
 		}
 	}
+
 	_pub.publish(rotor_vel_msg);
 }
 
- int main(int argc, char **argv)
+int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "mc_mixer");
-  MultirotorMixer mixer;
-  ros::spin();
+	ros::init(argc, argv, "mc_mixer");
+	MultirotorMixer mixer;
+	ros::spin();
 
-  return 0;
+	return 0;
 }
 
 void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)

From 74af4807a1a7adca0a6f6b6c668c48808ab858ec Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 08:26:04 +0100
Subject: [PATCH 271/416] Matrix.hpp: remove wrong and correct formatting
 changes which are not on master for clarity

---
 src/lib/mathlib/math/Matrix.hpp | 21 ++++++++++++---------
 1 file changed, 12 insertions(+), 9 deletions(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 68f0a777db..f6f4fc5ead 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -83,9 +83,10 @@ public:
 	 * trivial ctor
 	 * Initializes the elements to zero.
 	 */
-	MatrixBase() :
-		data {},
-	arm_mat {M, N, &data[0][0]} {
+	MatrixBase() : 
+		data{},
+		arm_mat{M, N, &data[0][0]}
+	{
 	}
 
 	virtual ~MatrixBase() {};
@@ -94,17 +95,20 @@ public:
 	 * copyt ctor
 	 */
 	MatrixBase(const MatrixBase<M, N> &m) :
-		arm_mat {M, N, &data[0][0]} {
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, m.data, sizeof(data));
 	}
 
 	MatrixBase(const float *d) :
-		arm_mat {M, N, &data[0][0]} {
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 
-	MatrixBase(const float d[M][N]) :
-		arm_mat {M, N, &data[0][0]} {
+	MatrixBase(const float d[M][N]) : 
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 
@@ -165,9 +169,8 @@ public:
 	bool operator ==(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j]) {
+				if (data[i][j] != m.data[i][j])
 					return false;
-				}
 
 		return true;
 	}

From f13f41f704e77ec5259fe2cc8efc440b052db1f7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 09:08:50 +0100
Subject: [PATCH 272/416] point uavcan submodule to same commit as master

---
 src/lib/uavcan | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/uavcan b/src/lib/uavcan
index 6a19364898..1efd244275 160000
--- a/src/lib/uavcan
+++ b/src/lib/uavcan
@@ -1 +1 @@
-Subproject commit 6a193648985adab9665e31a9460be04bc91d8965
+Subproject commit 1efd24427539fa332a15151143466ec760fa5fff

From 94ab82ba4074c52c612e5475bbabc57482b24a59 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 09:14:21 +0100
Subject: [PATCH 273/416] R_adapted.data is 2d, making this more obvious

---
 .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index d68f12c8e8..f0e02c3311 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -619,7 +619,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitch_sec   = euler_angles_sec(1);
 					att.yaw_sec     = euler_angles_sec(2);
 
-					memcpy(&att.R_sec[0], &R_adapted.data[0], sizeof(att.R_sec));
+					memcpy(&att.R_sec[0], &R_adapted.data[0][0], sizeof(att.R_sec));
 
 					att.rollspeed_sec  = -x_aposteriori[2];
 					att.pitchspeed_sec = x_aposteriori[1];

From f86c0ed892c0b63af11d23e860c815d1c087ff8e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 09:17:22 +0100
Subject: [PATCH 274/416] remove fw_att_control base classes: as long as they
 are not integrated into fw_att_control_main they are useless

---
 .../fw_att_control/fw_att_control_base.cpp    | 325 ------------------
 .../fw_att_control/fw_att_control_base.h      | 151 --------
 2 files changed, 476 deletions(-)
 delete mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp
 delete mode 100644 src/modules/fw_att_control/fw_att_control_base.h

diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp
deleted file mode 100644
index f543c02f9c..0000000000
--- a/src/modules/fw_att_control/fw_att_control_base.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-* notice, this list of conditions and the following disclaimer in
-* the documentation and/or other materials provided with the
-* distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-* used to endorse or promote products derived from this software
-* without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
- * @file mc_att_control_base.cpp
- *
- * @author Roman Bapst <bapstr@ethz.ch>
- *
- */
-
-#include "fw_att_control_base.h"
-#include <math.h>
-#include <mathlib/mathlib.h>
-#include <drivers/drv_hrt.h>
-
-using namespace std;
-
-FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
-
-		_task_should_exit(false), _task_running(false), _control_task(-1),
-
-		/* performance counters */
-		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
-				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
-				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
-		/* states */
-		_setpoint_valid(false), _debug(false) {
-	/* safely initialize structs */
-	_att = {};
-	_att_sp = {};
-	_manual = {};
-	_airspeed = {};
-	_vcontrol_mode = {};
-	_actuators = {};
-	_actuators_airframe = {};
-	_global_pos = {};
-	_vehicle_status = {};
-
-}
-
-FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() {
-
-}
-
-void FixedwingAttitudeControlBase::control_attitude() {
-	bool lock_integrator = false;
-	static int loop_counter = 0;
-	/* scale around tuning airspeed */
-
-	float airspeed;
-
-	/* if airspeed is not updating, we assume the normal average speed */
-	if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s)
-			|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
-		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);
-	}
-
-	/*
-	 * For scaling our actuators using anything less than the min (close to stall)
-	 * speed doesn't make any sense - its the strongest reasonable deflection we
-	 * want to do in flight and its the baseline a human pilot would choose.
-	 *
-	 * Forcing the scaling to this value allows reasonable handheld tests.
-	 */
-
-	float airspeed_scaling = _parameters.airspeed_trim
-			/ ((airspeed < _parameters.airspeed_min) ?
-					_parameters.airspeed_min : airspeed);
-
-	float roll_sp = _parameters.rollsp_offset_rad;
-	float pitch_sp = _parameters.pitchsp_offset_rad;
-	float throttle_sp = 0.0f;
-
-	if (_vcontrol_mode.flag_control_velocity_enabled
-			|| _vcontrol_mode.flag_control_position_enabled) {
-		/* read in attitude setpoint from attitude setpoint uorb topic */
-		roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-		pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
-		throttle_sp = _att_sp.thrust;
-
-		/* reset integrals where needed */
-		if (_att_sp.roll_reset_integral) {
-			_roll_ctrl.reset_integrator();
-		}
-		if (_att_sp.pitch_reset_integral) {
-			_pitch_ctrl.reset_integrator();
-		}
-		if (_att_sp.yaw_reset_integral) {
-			_yaw_ctrl.reset_integrator();
-		}
-	} else {
-		/*
-		 * Scale down roll and pitch as the setpoints are radians
-		 * and a typical remote can only do around 45 degrees, the mapping is
-		 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
-		 *
-		 * With this mapping the stick angle is a 1:1 representation of
-		 * the commanded attitude.
-		 *
-		 * The trim gets subtracted here from the manual setpoint to get
-		 * the intended attitude setpoint. Later, after the rate control step the
-		 * trim is added again to the control signal.
-		 */
-		roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
-				+ _parameters.rollsp_offset_rad;
-		pitch_sp = -(_manual.x * _parameters.man_pitch_max
-				- _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
-		throttle_sp = _manual.z;
-		_actuators.control[4] = _manual.flaps;
-
-		/*
-		 * in manual mode no external source should / does emit attitude setpoints.
-		 * emit the manual setpoint here to allow attitude controller tuning
-		 * in attitude control mode.
-		 */
-		struct vehicle_attitude_setpoint_s att_sp;
-		att_sp.timestamp = hrt_absolute_time();
-		att_sp.roll_body = roll_sp;
-		att_sp.pitch_body = pitch_sp;
-		att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
-		att_sp.thrust = throttle_sp;
-
-	}
-
-	/* If the aircraft is on ground reset the integrators */
-	if (_vehicle_status.condition_landed) {
-		_roll_ctrl.reset_integrator();
-		_pitch_ctrl.reset_integrator();
-		_yaw_ctrl.reset_integrator();
-	}
-
-	/* 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 = _att.R[0][0] * _global_pos.vel_n
-				+ _att.R[1][0] * _global_pos.vel_e
-				+ _att.R[2][0] * _global_pos.vel_d;
-		speed_body_v = _att.R[0][1] * _global_pos.vel_n
-				+ _att.R[1][1] * _global_pos.vel_e
-				+ _att.R[2][1] * _global_pos.vel_d;
-		speed_body_w = _att.R[0][2] * _global_pos.vel_n
-				+ _att.R[1][2] * _global_pos.vel_e
-				+ _att.R[2][2] * _global_pos.vel_d;
-	} else {
-		if (_debug && loop_counter % 10 == 0) {
-			warnx("Did not get a valid R\n");
-		}
-	}
-
-	/* Run attitude controllers */
-	if (isfinite(roll_sp) && isfinite(pitch_sp)) {
-		_roll_ctrl.control_attitude(roll_sp, _att.roll);
-		_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
-		_yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u,
-				speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(),
-				_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
-
-		/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
-		float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed,
-				_att.yawspeed, _yaw_ctrl.get_desired_rate(),
-				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
-				airspeed_scaling, lock_integrator);
-		_actuators.control[0] =
-				(isfinite(roll_u)) ?
-						roll_u + _parameters.trim_roll : _parameters.trim_roll;
-		if (!isfinite(roll_u)) {
-			_roll_ctrl.reset_integrator();
-			perf_count(_nonfinite_output_perf);
-
-			if (_debug && loop_counter % 10 == 0) {
-				warnx("roll_u %.4f", (double) roll_u);
-			}
-		}
-
-		float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
-				_att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(),
-				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
-				airspeed_scaling, lock_integrator);
-		_actuators.control[1] =
-				(isfinite(pitch_u)) ?
-						pitch_u + _parameters.trim_pitch :
-						_parameters.trim_pitch;
-		if (!isfinite(pitch_u)) {
-			_pitch_ctrl.reset_integrator();
-			perf_count(_nonfinite_output_perf);
-			if (_debug && loop_counter % 10 == 0) {
-				warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
-						" airspeed %.4f, airspeed_scaling %.4f,"
-						" roll_sp %.4f, pitch_sp %.4f,"
-						" _roll_ctrl.get_desired_rate() %.4f,"
-						" _pitch_ctrl.get_desired_rate() %.4f"
-						" att_sp.roll_body %.4f", (double) pitch_u,
-						(double) _yaw_ctrl.get_desired_rate(),
-						(double) airspeed, (double) airspeed_scaling,
-						(double) roll_sp, (double) pitch_sp,
-						(double) _roll_ctrl.get_desired_rate(),
-						(double) _pitch_ctrl.get_desired_rate(),
-						(double) _att_sp.roll_body);
-			}
-		}
-
-		float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
-				_att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(),
-				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
-				airspeed_scaling, lock_integrator);
-		_actuators.control[2] =
-				(isfinite(yaw_u)) ?
-						yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
-		if (!isfinite(yaw_u)) {
-			_yaw_ctrl.reset_integrator();
-			perf_count(_nonfinite_output_perf);
-			if (_debug && loop_counter % 10 == 0) {
-				warnx("yaw_u %.4f", (double) yaw_u);
-			}
-		}
-
-		/* throttle passed through */
-		_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
-		if (!isfinite(throttle_sp)) {
-			if (_debug && loop_counter % 10 == 0) {
-				warnx("throttle_sp %.4f", (double) throttle_sp);
-			}
-		}
-	} else {
-		perf_count(_nonfinite_input_perf);
-		if (_debug && loop_counter % 10 == 0) {
-			warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
-					(double) roll_sp, (double) pitch_sp);
-		}
-	}
-
-}
-
-void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
-	// watch out, still need to see where we modify attitude for the tailsitter case
-	math::Quaternion quat;
-	quat(0) = (float)attitude.w();
-	quat(1) = (float)attitude.x();
-	quat(2) = (float)attitude.y();
-	quat(3) = (float)attitude.z();
-
-	_att.q[0] = quat(0);
-	_att.q[1] = quat(1);
-	_att.q[2] = quat(2);
-	_att.q[3] = quat(3);
-
-	math::Matrix<3,3> Rot = quat.to_dcm();
-	_att.R[0][0] = Rot(0,0);
-	_att.R[1][0] = Rot(1,0);
-	_att.R[2][0] = Rot(2,0);
-	_att.R[0][1] = Rot(0,1);
-	_att.R[1][1] = Rot(1,1);
-	_att.R[2][1] = Rot(2,1);
-	_att.R[0][2] = Rot(0,2);
-	_att.R[1][2] = Rot(1,2);
-	_att.R[2][2] = Rot(2,2);
-
-	_att.R_valid = true;
-}
-void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
-	_att.rollspeed  = angular_rate(0);
-	_att.pitchspeed = angular_rate(1);
-	_att.yawspeed   = angular_rate(2);
-}
-void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
-	_att_sp.roll_body  = control_attitude_thrust_reference(0);
-	_att_sp.pitch_body = control_attitude_thrust_reference(1);
-	_att_sp.yaw_body   = control_attitude_thrust_reference(2);
-	_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
-
-	// setup rotation matrix
-	math::Matrix<3,3> Rot_sp;
-	Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
-	_att_sp.R_body[0][0] = Rot_sp(0,0);
-	_att_sp.R_body[1][0] = Rot_sp(1,0);
-	_att_sp.R_body[2][0] = Rot_sp(2,0);
-	_att_sp.R_body[0][1] = Rot_sp(0,1);
-	_att_sp.R_body[1][1] = Rot_sp(1,1);
-	_att_sp.R_body[2][1] = Rot_sp(2,1);
-	_att_sp.R_body[0][2] = Rot_sp(0,2);
-	_att_sp.R_body[1][2] = Rot_sp(1,2);
-	_att_sp.R_body[2][2] = Rot_sp(2,2);
-}
-void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
-	motor_inputs(0) = _actuators.control[0];
-	motor_inputs(1) = _actuators.control[1];
-	motor_inputs(2) = _actuators.control[2];
-	motor_inputs(3) = _actuators.control[3];
-}
diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h
deleted file mode 100644
index bde1b755f5..0000000000
--- a/src/modules/fw_att_control/fw_att_control_base.h
+++ /dev/null
@@ -1,151 +0,0 @@
-#ifndef FW_ATT_CONTROL_BASE_H_
-#define FW_ATT_CONTROL_BASE_H_
-
-/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-* notice, this list of conditions and the following disclaimer in
-* the documentation and/or other materials provided with the
-* distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-* used to endorse or promote products derived from this software
-* without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-		* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-		* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
- * @file fw_att_control_base.h
- * 
- * @author Roman Bapst <bapstr@ethz.ch>
- *
- */
-
-#include <ecl/attitude_fw/ecl_pitch_controller.h>
-#include <ecl/attitude_fw/ecl_roll_controller.h>
-#include <ecl/attitude_fw/ecl_yaw_controller.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>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_status.h>
-
-#include <systemlib/perf_counter.h>
-
-class FixedwingAttitudeControlBase
-{
-public:
-	/**
-	 * Constructor
-	 */
-	FixedwingAttitudeControlBase();
-
-	/**
-	 * Destructor
-	 */
-	~FixedwingAttitudeControlBase();
-
-
-protected:
-
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_control_task;			/**< task handle for sensor task */
-
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude 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 */
-	struct vehicle_global_position_s		_global_pos;		/**< global position */
-	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
-	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
-
-	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-	bool		_debug;				/**< if set to true, print debug output */
-
-	struct {
-		float tconst;
-		float p_p;
-		float p_d;
-		float p_i;
-		float p_ff;
-		float p_rmax_pos;
-		float p_rmax_neg;
-		float p_integrator_max;
-		float p_roll_feedforward;
-		float r_p;
-		float r_d;
-		float r_i;
-		float r_ff;
-		float r_integrator_max;
-		float r_rmax;
-		float y_p;
-		float y_i;
-		float y_d;
-		float y_ff;
-		float y_roll_feedforward;
-		float y_integrator_max;
-		float y_coordinated_min_speed;
-		float y_rmax;
-
-		float airspeed_min;
-		float airspeed_trim;
-		float airspeed_max;
-
-		float trim_roll;
-		float trim_pitch;
-		float trim_yaw;
-		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
-		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
-		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
-		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
-		float man_roll_max;						/**< Max Roll in rad */
-		float man_pitch_max;					/**< Max Pitch in rad */
-
-	}		_parameters;			/**< local copies of interesting parameters */
-
-	ECL_RollController				_roll_ctrl;
-	ECL_PitchController				_pitch_ctrl;
-	ECL_YawController				_yaw_ctrl;
-
-	void control_attitude();
-
-	// setters and getters for interface with euroc-gazebo simulator
-	void set_attitude(const Eigen::Quaternion<double> attitude);
-	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
-	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
-	void get_mixer_input(Eigen::Vector4d& motor_inputs);
-	
-};
-
-#endif /* FW_ATT_CONTROL_BASE_H_ */

From f960bbf529ba41580e06eb8a771600279f58e3c8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 10:54:09 +0100
Subject: [PATCH 275/416] bring back switch_pos_t

---
 src/modules/sensors/sensors.cpp | 8 ++++----
 src/modules/uORB/uORB.h         | 1 +
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f6ffedc6b5..0a27367d96 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -189,8 +189,8 @@ private:
 	/**
 	 * Get switch position for specified function.
 	 */
-	uint8_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
-	uint8_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
+	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
 
 	/**
 	 * Update paramters from RC channels if the functionality is activated and the
@@ -1666,7 +1666,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
 	}
 }
 
-uint8_t
+switch_pos_t
 Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
@@ -1687,7 +1687,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi
 	}
 }
 
-uint8_t
+switch_pos_t
 Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 {
 	if (_rc.function[func] >= 0) {
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 707ff1efdc..f19fbba7c5 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -314,5 +314,6 @@ typedef uint8_t arming_state_t;
 typedef uint8_t main_state_t;
 typedef uint8_t hil_state_t;
 typedef uint8_t navigation_state_t;
+typedef uint8_t switch_pos_t;
 
 #endif /* _UORB_UORB_H */

From 42430fb5efae6801e63106df3a33de6150ff5095 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 11:29:16 +0100
Subject: [PATCH 276/416] remove .catkin_workspace file

---
 .catkin_workspace | 1 -
 1 file changed, 1 deletion(-)
 delete mode 100644 .catkin_workspace

diff --git a/.catkin_workspace b/.catkin_workspace
deleted file mode 100644
index 52fd97e7ea..0000000000
--- a/.catkin_workspace
+++ /dev/null
@@ -1 +0,0 @@
-# This file currently only serves to mark the location of a catkin workspace for tool integration

From bb0d04212f31051fe4b71848c0a3b8adeb9b6e39 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 8 Jan 2015 11:33:59 +0100
Subject: [PATCH 277/416] remove copy of std_msgs

---
 msg/std_msgs/Bool.msg                |  1 -
 msg/std_msgs/Byte.msg                |  1 -
 msg/std_msgs/ByteMultiArray.msg      |  6 ------
 msg/std_msgs/Char.msg                |  1 -
 msg/std_msgs/ColorRGBA.msg           |  4 ----
 msg/std_msgs/Duration.msg            |  1 -
 msg/std_msgs/Empty.msg               |  0
 msg/std_msgs/Float32.msg             |  1 -
 msg/std_msgs/Float32MultiArray.msg   |  6 ------
 msg/std_msgs/Float64.msg             |  1 -
 msg/std_msgs/Float64MultiArray.msg   |  6 ------
 msg/std_msgs/Header.msg              | 15 ---------------
 msg/std_msgs/Int16.msg               |  1 -
 msg/std_msgs/Int16MultiArray.msg     |  6 ------
 msg/std_msgs/Int32.msg               |  1 -
 msg/std_msgs/Int32MultiArray.msg     |  6 ------
 msg/std_msgs/Int64.msg               |  1 -
 msg/std_msgs/Int64MultiArray.msg     |  6 ------
 msg/std_msgs/Int8.msg                |  1 -
 msg/std_msgs/Int8MultiArray.msg      |  6 ------
 msg/std_msgs/MultiArrayDimension.msg |  3 ---
 msg/std_msgs/MultiArrayLayout.msg    | 26 --------------------------
 msg/std_msgs/String.msg              |  1 -
 msg/std_msgs/Time.msg                |  1 -
 msg/std_msgs/UInt16.msg              |  1 -
 msg/std_msgs/UInt16MultiArray.msg    |  6 ------
 msg/std_msgs/UInt32.msg              |  1 -
 msg/std_msgs/UInt32MultiArray.msg    |  6 ------
 msg/std_msgs/UInt64.msg              |  1 -
 msg/std_msgs/UInt64MultiArray.msg    |  6 ------
 msg/std_msgs/UInt8.msg               |  1 -
 msg/std_msgs/UInt8MultiArray.msg     |  6 ------
 32 files changed, 130 deletions(-)
 delete mode 100644 msg/std_msgs/Bool.msg
 delete mode 100644 msg/std_msgs/Byte.msg
 delete mode 100644 msg/std_msgs/ByteMultiArray.msg
 delete mode 100644 msg/std_msgs/Char.msg
 delete mode 100644 msg/std_msgs/ColorRGBA.msg
 delete mode 100644 msg/std_msgs/Duration.msg
 delete mode 100644 msg/std_msgs/Empty.msg
 delete mode 100644 msg/std_msgs/Float32.msg
 delete mode 100644 msg/std_msgs/Float32MultiArray.msg
 delete mode 100644 msg/std_msgs/Float64.msg
 delete mode 100644 msg/std_msgs/Float64MultiArray.msg
 delete mode 100644 msg/std_msgs/Header.msg
 delete mode 100644 msg/std_msgs/Int16.msg
 delete mode 100644 msg/std_msgs/Int16MultiArray.msg
 delete mode 100644 msg/std_msgs/Int32.msg
 delete mode 100644 msg/std_msgs/Int32MultiArray.msg
 delete mode 100644 msg/std_msgs/Int64.msg
 delete mode 100644 msg/std_msgs/Int64MultiArray.msg
 delete mode 100644 msg/std_msgs/Int8.msg
 delete mode 100644 msg/std_msgs/Int8MultiArray.msg
 delete mode 100644 msg/std_msgs/MultiArrayDimension.msg
 delete mode 100644 msg/std_msgs/MultiArrayLayout.msg
 delete mode 100644 msg/std_msgs/String.msg
 delete mode 100644 msg/std_msgs/Time.msg
 delete mode 100644 msg/std_msgs/UInt16.msg
 delete mode 100644 msg/std_msgs/UInt16MultiArray.msg
 delete mode 100644 msg/std_msgs/UInt32.msg
 delete mode 100644 msg/std_msgs/UInt32MultiArray.msg
 delete mode 100644 msg/std_msgs/UInt64.msg
 delete mode 100644 msg/std_msgs/UInt64MultiArray.msg
 delete mode 100644 msg/std_msgs/UInt8.msg
 delete mode 100644 msg/std_msgs/UInt8MultiArray.msg

diff --git a/msg/std_msgs/Bool.msg b/msg/std_msgs/Bool.msg
deleted file mode 100644
index f7cabb94fc..0000000000
--- a/msg/std_msgs/Bool.msg
+++ /dev/null
@@ -1 +0,0 @@
-bool data
\ No newline at end of file
diff --git a/msg/std_msgs/Byte.msg b/msg/std_msgs/Byte.msg
deleted file mode 100644
index d993b34555..0000000000
--- a/msg/std_msgs/Byte.msg
+++ /dev/null
@@ -1 +0,0 @@
-byte data
diff --git a/msg/std_msgs/ByteMultiArray.msg b/msg/std_msgs/ByteMultiArray.msg
deleted file mode 100644
index bb00bd348b..0000000000
--- a/msg/std_msgs/ByteMultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-byte[]            data          # array of data
-
diff --git a/msg/std_msgs/Char.msg b/msg/std_msgs/Char.msg
deleted file mode 100644
index 39a1d46a97..0000000000
--- a/msg/std_msgs/Char.msg
+++ /dev/null
@@ -1 +0,0 @@
-char data
\ No newline at end of file
diff --git a/msg/std_msgs/ColorRGBA.msg b/msg/std_msgs/ColorRGBA.msg
deleted file mode 100644
index 182dbc8349..0000000000
--- a/msg/std_msgs/ColorRGBA.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-float32 r
-float32 g
-float32 b
-float32 a
diff --git a/msg/std_msgs/Duration.msg b/msg/std_msgs/Duration.msg
deleted file mode 100644
index f13931ec8a..0000000000
--- a/msg/std_msgs/Duration.msg
+++ /dev/null
@@ -1 +0,0 @@
-duration data
diff --git a/msg/std_msgs/Empty.msg b/msg/std_msgs/Empty.msg
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/msg/std_msgs/Float32.msg b/msg/std_msgs/Float32.msg
deleted file mode 100644
index e89740534b..0000000000
--- a/msg/std_msgs/Float32.msg
+++ /dev/null
@@ -1 +0,0 @@
-float32 data
\ No newline at end of file
diff --git a/msg/std_msgs/Float32MultiArray.msg b/msg/std_msgs/Float32MultiArray.msg
deleted file mode 100644
index 915830846d..0000000000
--- a/msg/std_msgs/Float32MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-float32[]         data          # array of data
-
diff --git a/msg/std_msgs/Float64.msg b/msg/std_msgs/Float64.msg
deleted file mode 100644
index cd09d39b8c..0000000000
--- a/msg/std_msgs/Float64.msg
+++ /dev/null
@@ -1 +0,0 @@
-float64 data
\ No newline at end of file
diff --git a/msg/std_msgs/Float64MultiArray.msg b/msg/std_msgs/Float64MultiArray.msg
deleted file mode 100644
index 0a13b928fd..0000000000
--- a/msg/std_msgs/Float64MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-float64[]         data          # array of data
-
diff --git a/msg/std_msgs/Header.msg b/msg/std_msgs/Header.msg
deleted file mode 100644
index f90d622ea5..0000000000
--- a/msg/std_msgs/Header.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-# Standard metadata for higher-level stamped data types.
-# This is generally used to communicate timestamped data 
-# in a particular coordinate frame.
-# 
-# sequence ID: consecutively increasing ID 
-uint32 seq
-#Two-integer timestamp that is expressed as:
-# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
-# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
-# time-handling sugar is provided by the client library
-time stamp
-#Frame this data is associated with
-# 0: no frame
-# 1: global frame
-string frame_id
diff --git a/msg/std_msgs/Int16.msg b/msg/std_msgs/Int16.msg
deleted file mode 100644
index c4389faf70..0000000000
--- a/msg/std_msgs/Int16.msg
+++ /dev/null
@@ -1 +0,0 @@
-int16 data
diff --git a/msg/std_msgs/Int16MultiArray.msg b/msg/std_msgs/Int16MultiArray.msg
deleted file mode 100644
index d2ddea1d1d..0000000000
--- a/msg/std_msgs/Int16MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-int16[]           data          # array of data
-
diff --git a/msg/std_msgs/Int32.msg b/msg/std_msgs/Int32.msg
deleted file mode 100644
index 0ecfe35f5f..0000000000
--- a/msg/std_msgs/Int32.msg
+++ /dev/null
@@ -1 +0,0 @@
-int32 data
\ No newline at end of file
diff --git a/msg/std_msgs/Int32MultiArray.msg b/msg/std_msgs/Int32MultiArray.msg
deleted file mode 100644
index af60abda3a..0000000000
--- a/msg/std_msgs/Int32MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-int32[]           data          # array of data
-
diff --git a/msg/std_msgs/Int64.msg b/msg/std_msgs/Int64.msg
deleted file mode 100644
index 6961e00f52..0000000000
--- a/msg/std_msgs/Int64.msg
+++ /dev/null
@@ -1 +0,0 @@
-int64 data
\ No newline at end of file
diff --git a/msg/std_msgs/Int64MultiArray.msg b/msg/std_msgs/Int64MultiArray.msg
deleted file mode 100644
index f4f35e171b..0000000000
--- a/msg/std_msgs/Int64MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-int64[]           data          # array of data
-
diff --git a/msg/std_msgs/Int8.msg b/msg/std_msgs/Int8.msg
deleted file mode 100644
index 1e42e554fe..0000000000
--- a/msg/std_msgs/Int8.msg
+++ /dev/null
@@ -1 +0,0 @@
-int8 data
diff --git a/msg/std_msgs/Int8MultiArray.msg b/msg/std_msgs/Int8MultiArray.msg
deleted file mode 100644
index a59a37259e..0000000000
--- a/msg/std_msgs/Int8MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-int8[]            data          # array of data
-
diff --git a/msg/std_msgs/MultiArrayDimension.msg b/msg/std_msgs/MultiArrayDimension.msg
deleted file mode 100644
index 08240462c4..0000000000
--- a/msg/std_msgs/MultiArrayDimension.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-string label   # label of given dimension
-uint32 size    # size of given dimension (in type units)
-uint32 stride  # stride of given dimension
\ No newline at end of file
diff --git a/msg/std_msgs/MultiArrayLayout.msg b/msg/std_msgs/MultiArrayLayout.msg
deleted file mode 100644
index 5437f8542a..0000000000
--- a/msg/std_msgs/MultiArrayLayout.msg
+++ /dev/null
@@ -1,26 +0,0 @@
-# The multiarray declares a generic multi-dimensional array of a
-# particular data type.  Dimensions are ordered from outer most
-# to inner most.
-
-MultiArrayDimension[] dim # Array of dimension properties
-uint32 data_offset        # padding bytes at front of data
-
-# Accessors should ALWAYS be written in terms of dimension stride
-# and specified outer-most dimension first.
-# 
-# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]
-#
-# A standard, 3-channel 640x480 image with interleaved color channels
-# would be specified as:
-#
-# dim[0].label  = "height"
-# dim[0].size   = 480
-# dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)
-# dim[1].label  = "width"
-# dim[1].size   = 640
-# dim[1].stride = 3*640 = 1920
-# dim[2].label  = "channel"
-# dim[2].size   = 3
-# dim[2].stride = 3
-#
-# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.
\ No newline at end of file
diff --git a/msg/std_msgs/String.msg b/msg/std_msgs/String.msg
deleted file mode 100644
index ae721739e8..0000000000
--- a/msg/std_msgs/String.msg
+++ /dev/null
@@ -1 +0,0 @@
-string data
diff --git a/msg/std_msgs/Time.msg b/msg/std_msgs/Time.msg
deleted file mode 100644
index 7f8f721711..0000000000
--- a/msg/std_msgs/Time.msg
+++ /dev/null
@@ -1 +0,0 @@
-time data
diff --git a/msg/std_msgs/UInt16.msg b/msg/std_msgs/UInt16.msg
deleted file mode 100644
index 87d0c44eb5..0000000000
--- a/msg/std_msgs/UInt16.msg
+++ /dev/null
@@ -1 +0,0 @@
-uint16 data
diff --git a/msg/std_msgs/UInt16MultiArray.msg b/msg/std_msgs/UInt16MultiArray.msg
deleted file mode 100644
index f38970b656..0000000000
--- a/msg/std_msgs/UInt16MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-uint16[]            data        # array of data
-
diff --git a/msg/std_msgs/UInt32.msg b/msg/std_msgs/UInt32.msg
deleted file mode 100644
index b6c696b421..0000000000
--- a/msg/std_msgs/UInt32.msg
+++ /dev/null
@@ -1 +0,0 @@
-uint32 data
\ No newline at end of file
diff --git a/msg/std_msgs/UInt32MultiArray.msg b/msg/std_msgs/UInt32MultiArray.msg
deleted file mode 100644
index b2bb0771f0..0000000000
--- a/msg/std_msgs/UInt32MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-uint32[]          data          # array of data
-
diff --git a/msg/std_msgs/UInt64.msg b/msg/std_msgs/UInt64.msg
deleted file mode 100644
index 2eb1afad37..0000000000
--- a/msg/std_msgs/UInt64.msg
+++ /dev/null
@@ -1 +0,0 @@
-uint64 data
\ No newline at end of file
diff --git a/msg/std_msgs/UInt64MultiArray.msg b/msg/std_msgs/UInt64MultiArray.msg
deleted file mode 100644
index 30d0cd9285..0000000000
--- a/msg/std_msgs/UInt64MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-uint64[]          data          # array of data
-
diff --git a/msg/std_msgs/UInt8.msg b/msg/std_msgs/UInt8.msg
deleted file mode 100644
index 5eefd870db..0000000000
--- a/msg/std_msgs/UInt8.msg
+++ /dev/null
@@ -1 +0,0 @@
-uint8 data
diff --git a/msg/std_msgs/UInt8MultiArray.msg b/msg/std_msgs/UInt8MultiArray.msg
deleted file mode 100644
index 31f7d6a213..0000000000
--- a/msg/std_msgs/UInt8MultiArray.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Please look at the MultiArrayLayout message definition for
-# documentation on all multiarrays.
-
-MultiArrayLayout  layout        # specification of data layout
-uint8[]           data          # array of data
-

From 2b43e0fc3072c246de9cd3dd36e3b803c7b81a9c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 9 Jan 2015 09:15:48 +0100
Subject: [PATCH 278/416] autstart for mc_att_control_m

---
 ROMFS/px4fmu_common/init.d/rc.mc_apps | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 268eb9bba0..2248b0bcd3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -8,5 +8,10 @@ attitude_estimator_ekf start
 #ekf_att_pos_estimator start
 position_estimator_inav start
 
-mc_att_control start
+if mc_att_control start
+then
+else
+	# try the multiplatform version
+	mc_att_control_m start
+fi
 mc_pos_control start

From c2cc247e76de53664a3826f04d251f69df6a8fab Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 9 Jan 2015 09:18:33 +0100
Subject: [PATCH 279/416] renamed mc_att_control_multiplatform to
 mc_att_control_m

---
 .../mc_att_control_main.cpp                      | 16 ++++++++--------
 .../mc_att_control_multiplatform/module.mk       |  2 +-
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
index e2f2f2530c..67ebe64cdc 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -65,7 +65,7 @@ bool task_should_exit = false;
 
 using namespace px4;
 
-PX4_MAIN_FUNCTION(mc_att_control_multiplatform);
+PX4_MAIN_FUNCTION(mc_att_control_m);
 
 #if !defined(__PX4_ROS)
 /**
@@ -74,11 +74,11 @@ PX4_MAIN_FUNCTION(mc_att_control_multiplatform);
  * @ingroup apps
  */
 
-extern "C" __EXPORT int mc_att_control_multiplatform_main(int argc, char *argv[]);
-int mc_att_control_multiplatform_main(int argc, char *argv[])
+extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
+int mc_att_control_m_main(int argc, char *argv[])
 {
 	if (argc < 1) {
-		errx(1, "usage: mc_att_control {start|stop|status}");
+		errx(1, "usage: mc_att_control_m {start|stop|status}");
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -91,11 +91,11 @@ int mc_att_control_multiplatform_main(int argc, char *argv[])
 
 		task_should_exit = false;
 
-		daemon_task = task_spawn_cmd("mc_att_control",
+		daemon_task = task_spawn_cmd("mc_att_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       3000,
-				       mc_att_control_multiplatform_task_main,
+				       mc_att_control_m_task_main,
 					(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 
 		exit(0);
@@ -122,9 +122,9 @@ int mc_att_control_multiplatform_main(int argc, char *argv[])
 }
 #endif
 
-PX4_MAIN_FUNCTION(mc_att_control_multiplatform)
+PX4_MAIN_FUNCTION(mc_att_control_m)
 {
-	px4::init(argc, argv, "mc_att_control_multiplatform");
+	px4::init(argc, argv, "mc_att_control_m");
 
 	PX4_INFO("starting");
 	MulticopterAttitudeControl attctl;
diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk
index 7925b83450..c61ba18b42 100644
--- a/src/modules/mc_att_control_multiplatform/module.mk
+++ b/src/modules/mc_att_control_multiplatform/module.mk
@@ -35,7 +35,7 @@
 # Multirotor attitude controller (vector based, no Euler singularities)
 #
 
-MODULE_COMMAND	= mc_att_control_multiplatform
+MODULE_COMMAND	= mc_att_control_m
 
 SRCS		= mc_att_control_main.cpp \
 		  mc_att_control.cpp \

From aa8555c02b48ec5864a4dbbec6f38fbae6f73c7c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 9 Jan 2015 16:56:15 +0100
Subject: [PATCH 280/416] add house world launch file

---
 launch/gazebo_multicopter_house_world.launch | 6 ++++++
 1 file changed, 6 insertions(+)
 create mode 100644 launch/gazebo_multicopter_house_world.launch

diff --git a/launch/gazebo_multicopter_house_world.launch b/launch/gazebo_multicopter_house_world.launch
new file mode 100644
index 0000000000..ce5aca357d
--- /dev/null
+++ b/launch/gazebo_multicopter_house_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/vtol_house_world_with_joy.launch" />
+<include file="$(find px4)/launch/multicopter.launch" />
+
+</launch>

From 6580d66d45cfb4b251e3b0f32b61161ee0d14ecb Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 10 Jan 2015 19:14:23 +0100
Subject: [PATCH 281/416] ros sim: use ardrone model

---
 launch/gazebo_multicopter.launch                            | 2 +-
 launch/multicopter.launch                                   | 6 +++---
 .../ros/nodes/attitude_estimator/attitude_estimator.cpp     | 6 +++---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp               | 2 +-
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_multicopter.launch
index febf7bdc07..8091e3ff2b 100644
--- a/launch/gazebo_multicopter.launch
+++ b/launch/gazebo_multicopter.launch
@@ -1,6 +1,6 @@
 <launch>
 
-<include file="$(find mav_gazebo)/launch/vtol_empty_world_with_joy.launch" />
+<include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" />
 <include file="$(find px4)/launch/multicopter.launch" />
 
 </launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 9956c871d9..0a4b8c26df 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -14,9 +14,9 @@
 	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
 	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
 	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
-	<param name="MC_YAW_FF" type="double" value="0.0" />
-	<param name="MC_YAW_P" type="double" value="1.0" />
-	<param name="MC_YAWRATE_P" type="double" value="0.2" />
+	<param name="MC_YAW_FF" type="double" value="0" />
+	<param name="MC_YAW_P" type="double" value="5.0" />
+	<param name="MC_YAWRATE_P" type="double" value="0.5" />
 	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
 	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
 </group>
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index ce863d10e3..9203343633 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -47,7 +47,7 @@
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
 	// _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
-	_sub_imu(_n.subscribe("/vtol/imu", 1, &AttitudeEstimator::ImuCallback, this)),
+	_sub_imu(_n.subscribe("/ardrone/imu", 1, &AttitudeEstimator::ImuCallback, this)),
 	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
 {
 }
@@ -60,7 +60,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 
 	/* Convert quaternion to rotation matrix */
 	math::Quaternion quat;
-	//XXX: search for vtol or other (other than 'plane') vehicle here
+	//XXX: search for ardrone or other (other than 'plane') vehicle here
 	int index = 1;
 	quat(0) = (float)msg->pose[index].orientation.w;
 	quat(1) = (float)msg->pose[index].orientation.x;
@@ -103,7 +103,7 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
 
 	/* Convert quaternion to rotation matrix */
 	math::Quaternion quat;
-	//XXX: search for vtol or other (other than 'plane') vehicle here
+	//XXX: search for ardrone or other (other than 'plane') vehicle here
 	int index = 1;
 	quat(0) = (float)msg->orientation.w;
 	quat(1) = (float)msg->orientation.x;
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index e2180af41b..3867d448e6 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -111,7 +111,7 @@ const MultirotorMixer::Rotor *_config_index[3] = {
 MultirotorMixer::MultirotorMixer():
 	_n(),
 	_rotor_count(4),
-	_rotors(_config_index[2]) //XXX + eurocconfig hardcoded
+	_rotors(_config_index[0]) //XXX hardcoded
 {
 	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);

From 8f8daf6594a066919d84fc5e79cd264a23dadafa Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 10 Jan 2015 19:15:01 +0100
Subject: [PATCH 282/416] ros: house_world launch file

---
 launch/gazebo_multicopter_house_world.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/launch/gazebo_multicopter_house_world.launch b/launch/gazebo_multicopter_house_world.launch
index ce5aca357d..7b6be75acb 100644
--- a/launch/gazebo_multicopter_house_world.launch
+++ b/launch/gazebo_multicopter_house_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
-<include file="$(find mav_gazebo)/launch/vtol_house_world_with_joy.launch" />
+<include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" />
 <include file="$(find px4)/launch/multicopter.launch" />
 
 </launch>

From 2669c699c7974b9f1be5a0beb678dac724dfc909 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 13 Jan 2015 09:12:49 +0100
Subject: [PATCH 283/416] ros attitude estimator dummy: fix topic name

---
 .../ros/nodes/attitude_estimator/attitude_estimator.cpp         | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index 9203343633..bcee0b4799 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -47,7 +47,7 @@
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
 	// _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
-	_sub_imu(_n.subscribe("/ardrone/imu", 1, &AttitudeEstimator::ImuCallback, this)),
+	_sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)),
 	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
 {
 }

From 1cff86b0b562301020973f354043f27272d29f5b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 13 Jan 2015 09:13:11 +0100
Subject: [PATCH 284/416] ros mixer: increase number of controls to default to
 fix undefined behaviour

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 3867d448e6..1073833c08 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -68,11 +68,11 @@ private:
 	unsigned _rotor_count;
 
 	struct {
-		float control[6];
+		float control[8];
 	} inputs;
 
 	struct  {
-		float control[6];
+		float control[8];
 	} outputs;
 
 	bool _armed;

From 0c82600b83cea4c47ae8810cf1b29c4c805f79cf Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 13 Jan 2015 11:46:50 +0100
Subject: [PATCH 285/416] rename launch files and add iris empty world launch
 file

---
 ...multicopter.launch => gazebo_ardrone_empty_world.launch} | 0
 ...house_world.launch => gazebo_ardrone_house_world.launch} | 0
 launch/gazebo_iris_empty_world.launch                       | 6 ++++++
 3 files changed, 6 insertions(+)
 rename launch/{gazebo_multicopter.launch => gazebo_ardrone_empty_world.launch} (100%)
 rename launch/{gazebo_multicopter_house_world.launch => gazebo_ardrone_house_world.launch} (100%)
 create mode 100644 launch/gazebo_iris_empty_world.launch

diff --git a/launch/gazebo_multicopter.launch b/launch/gazebo_ardrone_empty_world.launch
similarity index 100%
rename from launch/gazebo_multicopter.launch
rename to launch/gazebo_ardrone_empty_world.launch
diff --git a/launch/gazebo_multicopter_house_world.launch b/launch/gazebo_ardrone_house_world.launch
similarity index 100%
rename from launch/gazebo_multicopter_house_world.launch
rename to launch/gazebo_ardrone_house_world.launch
diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch
new file mode 100644
index 0000000000..20574e00d0
--- /dev/null
+++ b/launch/gazebo_iris_empty_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" />
+<include file="$(find px4)/launch/multicopter.launch" />
+
+</launch>

From dd2d53d7e32044d44ec37bf36b0c0a6949c4f662 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 13 Jan 2015 14:49:38 +0100
Subject: [PATCH 286/416] add outdoor launch file

---
 launch/gazebo_iris_outdoor_world.launch | 6 ++++++
 1 file changed, 6 insertions(+)
 create mode 100644 launch/gazebo_iris_outdoor_world.launch

diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch
new file mode 100644
index 0000000000..e7fd0b3953
--- /dev/null
+++ b/launch/gazebo_iris_outdoor_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" />
+<include file="$(find px4)/launch/multicopter.launch" />
+
+</launch>

From f6c0d2310d538e44c3764f3a13d80789ca34170e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 14 Jan 2015 07:41:14 +0100
Subject: [PATCH 287/416] ros mixer node: add w mixer

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 1073833c08..e66558fe2e 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -101,17 +101,24 @@ const MultirotorMixer::Rotor _config_quad_plus_euroc[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
+const MultirotorMixer::Rotor _config_quad_wide[] = {
+	{ -0.927184,  0.374607,  1.000000 },
+	{  0.777146, -0.629320,  1.000000 },
+	{  0.927184,  0.374607, -1.000000 },
+	{ -0.777146, -0.629320, -1.000000 },
+};
 
-const MultirotorMixer::Rotor *_config_index[3] = {
+const MultirotorMixer::Rotor *_config_index[4] = {
 	&_config_x[0],
 	&_config_quad_plus[0],
-	&_config_quad_plus_euroc[0]
+	&_config_quad_plus_euroc[0],
+	&_config_quad_wide[0]
 };
 
 MultirotorMixer::MultirotorMixer():
 	_n(),
 	_rotor_count(4),
-	_rotors(_config_index[0]) //XXX hardcoded
+	_rotors(_config_index[3]) //XXX hardcoded
 {
 	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);

From 6b0d0aa2a5d2e9623ab2850396818679672715e9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 14 Jan 2015 11:27:32 +0100
Subject: [PATCH 288/416] ros: make mixer name a param

---
 launch/gazebo_ardrone_empty_world.launch      |  2 +-
 launch/gazebo_ardrone_house_world.launch      |  2 +-
 launch/gazebo_iris_empty_world.launch         |  2 +-
 launch/gazebo_iris_outdoor_world.launch       |  2 +-
 launch/multicopter.launch                     |  1 +
 launch/multicopter_w.launch                   |  9 +++++++++
 launch/multicopter_x.launch                   |  9 +++++++++
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 20 ++++++++++++++++++-
 8 files changed, 42 insertions(+), 5 deletions(-)
 create mode 100644 launch/multicopter_w.launch
 create mode 100644 launch/multicopter_x.launch

diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch
index 8091e3ff2b..00ef7ab459 100644
--- a/launch/gazebo_ardrone_empty_world.launch
+++ b/launch/gazebo_ardrone_empty_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter.launch" />
+<include file="$(find px4)/launch/multicopter_x.launch" />
 
 </launch>
diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch
index 7b6be75acb..f9ac1cac52 100644
--- a/launch/gazebo_ardrone_house_world.launch
+++ b/launch/gazebo_ardrone_house_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter.launch" />
+<include file="$(find px4)/launch/multicopter_x.launch" />
 
 </launch>
diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch
index 20574e00d0..22e0329783 100644
--- a/launch/gazebo_iris_empty_world.launch
+++ b/launch/gazebo_iris_empty_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter.launch" />
+<include file="$(find px4)/launch/multicopter_w.launch" />
 
 </launch>
diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch
index e7fd0b3953..88960b5ec5 100644
--- a/launch/gazebo_iris_outdoor_world.launch
+++ b/launch/gazebo_iris_outdoor_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter.launch" />
+<include file="$(find px4)/launch/multicopter_x.launch" />
 
 </launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 0a4b8c26df..79c3c36d97 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -19,6 +19,7 @@
 	<param name="MC_YAWRATE_P" type="double" value="0.5" />
 	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
 	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
+	<param name="mixer" type="string" value="x" />
 </group>
 
 </launch>
diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch
new file mode 100644
index 0000000000..f124082aa9
--- /dev/null
+++ b/launch/multicopter_w.launch
@@ -0,0 +1,9 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter.launch" />
+
+<group ns="px4_multicopter">
+	<param name="mixer" type="string" value="w" />
+</group>
+
+</launch>
diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch
new file mode 100644
index 0000000000..6355b87be9
--- /dev/null
+++ b/launch/multicopter_x.launch
@@ -0,0 +1,9 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter.launch" />
+
+<group ns="px4_multicopter">
+	<param name="mixer" type="string" value="x" />
+</group>
+
+</launch>
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index e66558fe2e..9954692bc1 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -41,6 +41,7 @@
 #include <px4.h>
 #include <lib/mathlib/mathlib.h>
 #include <mav_msgs/MotorSpeed.h>
+#include <string>
 
 class MultirotorMixer
 {
@@ -118,7 +119,7 @@ const MultirotorMixer::Rotor *_config_index[4] = {
 MultirotorMixer::MultirotorMixer():
 	_n(),
 	_rotor_count(4),
-	_rotors(_config_index[3]) //XXX hardcoded
+	_rotors(_config_index[0])
 {
 	_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
 	_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);
@@ -131,6 +132,10 @@ MultirotorMixer::MultirotorMixer():
 		_n.setParam("motor_offset_radps", 600.0);
 	}
 
+	if (!_n.hasParam("mixer")) {
+		_n.setParam("mixer", "x");
+	}
+
 	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this);
 }
 
@@ -206,6 +211,19 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro
 		inputs.control[i] = msg.control[i];
 	}
 
+	// switch mixer if necessary
+	std::string mixer_name;
+	_n.getParamCached("mixer", mixer_name);
+	if (mixer_name == "x") {
+		_rotors = _config_index[0];
+	} else if (mixer_name == "+") {
+		_rotors = _config_index[1];
+	} else if (mixer_name == "e") {
+		_rotors = _config_index[2];
+	} else if (mixer_name == "w") {
+		_rotors = _config_index[3];
+	}
+
 	// mix
 	mix();
 

From 3da1b701ed941effd92e7699e8e435a810300b49 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 14 Jan 2015 11:31:44 +0100
Subject: [PATCH 289/416] ros: add type specific use files than can be used by
 all toplevel launchfiles

---
 launch/ardrone.launch                    | 5 +++++
 launch/gazebo_ardrone_empty_world.launch | 2 +-
 launch/gazebo_ardrone_house_world.launch | 2 +-
 launch/gazebo_iris_empty_world.launch    | 2 +-
 launch/gazebo_iris_outdoor_world.launch  | 2 +-
 launch/iris.launch                       | 5 +++++
 6 files changed, 14 insertions(+), 4 deletions(-)
 create mode 100644 launch/ardrone.launch
 create mode 100644 launch/iris.launch

diff --git a/launch/ardrone.launch b/launch/ardrone.launch
new file mode 100644
index 0000000000..aa89bdd4e9
--- /dev/null
+++ b/launch/ardrone.launch
@@ -0,0 +1,5 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter_x.launch" />
+
+</launch>
diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch
index 00ef7ab459..395e70b006 100644
--- a/launch/gazebo_ardrone_empty_world.launch
+++ b/launch/gazebo_ardrone_empty_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter_x.launch" />
+<include file="$(find px4)/launch/ardrone.launch" />
 
 </launch>
diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch
index f9ac1cac52..f43c16b509 100644
--- a/launch/gazebo_ardrone_house_world.launch
+++ b/launch/gazebo_ardrone_house_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter_x.launch" />
+<include file="$(find px4)/launch/ardrone.launch" />
 
 </launch>
diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch
index 22e0329783..833aaf07aa 100644
--- a/launch/gazebo_iris_empty_world.launch
+++ b/launch/gazebo_iris_empty_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter_w.launch" />
+<include file="$(find px4)/launch/iris.launch" />
 
 </launch>
diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch
index 88960b5ec5..c2975a050d 100644
--- a/launch/gazebo_iris_outdoor_world.launch
+++ b/launch/gazebo_iris_outdoor_world.launch
@@ -1,6 +1,6 @@
 <launch>
 
 <include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" />
-<include file="$(find px4)/launch/multicopter_x.launch" />
+<include file="$(find px4)/launch/iris.launch" />
 
 </launch>
diff --git a/launch/iris.launch b/launch/iris.launch
new file mode 100644
index 0000000000..d9d08fae0c
--- /dev/null
+++ b/launch/iris.launch
@@ -0,0 +1,5 @@
+<launch>
+
+<include file="$(find px4)/launch/multicopter_w.launch" />
+
+</launch>

From 68914df4d0638c974e94657b35a8c12f71738d01 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 14 Jan 2015 11:34:55 +0100
Subject: [PATCH 290/416] ros: move params to type/frame launch files

---
 launch/ardrone.launch     | 14 ++++++++++++++
 launch/iris.launch        | 14 ++++++++++++++
 launch/multicopter.launch | 13 -------------
 3 files changed, 28 insertions(+), 13 deletions(-)

diff --git a/launch/ardrone.launch b/launch/ardrone.launch
index aa89bdd4e9..495388be5b 100644
--- a/launch/ardrone.launch
+++ b/launch/ardrone.launch
@@ -2,4 +2,18 @@
 
 <include file="$(find px4)/launch/multicopter_x.launch" />
 
+<group ns="px4_multicopter">
+	<param name="MC_ROLL_P" type="double" value="6.0" />
+	<param name="MC_PITCH_P" type="double" value="6.0" />
+	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
+	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
+	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
+	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
+	<param name="MC_YAW_FF" type="double" value="0" />
+	<param name="MC_YAW_P" type="double" value="5.0" />
+	<param name="MC_YAWRATE_P" type="double" value="0.5" />
+	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
+	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
+</group>
+
 </launch>
diff --git a/launch/iris.launch b/launch/iris.launch
index d9d08fae0c..81a137173a 100644
--- a/launch/iris.launch
+++ b/launch/iris.launch
@@ -2,4 +2,18 @@
 
 <include file="$(find px4)/launch/multicopter_w.launch" />
 
+<group ns="px4_multicopter">
+	<param name="MC_ROLL_P" type="double" value="6.0" />
+	<param name="MC_PITCH_P" type="double" value="6.0" />
+	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
+	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
+	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
+	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
+	<param name="MC_YAW_FF" type="double" value="0" />
+	<param name="MC_YAW_P" type="double" value="3.0" />
+	<param name="MC_YAWRATE_P" type="double" value="0.5" />
+	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
+	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
+</group>
+
 </launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 79c3c36d97..96ff3ad99c 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -7,19 +7,6 @@
 	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
-
-	<param name="MC_ROLL_P" type="double" value="6.0" />
-	<param name="MC_PITCH_P" type="double" value="6.0" />
-	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
-	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
-	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
-	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
-	<param name="MC_YAW_FF" type="double" value="0" />
-	<param name="MC_YAW_P" type="double" value="5.0" />
-	<param name="MC_YAWRATE_P" type="double" value="0.5" />
-	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
-	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
-	<param name="mixer" type="string" value="x" />
 </group>
 
 </launch>

From 1f26e1f5ebb0d0fffec98644d2a824f88415e9d7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 15 Jan 2015 12:36:30 +0100
Subject: [PATCH 291/416] remove hector from install script

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index c6f27653cf..d83039623e 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -23,9 +23,3 @@ source ~/.bashrc
 
 # get rosinstall
 sudo apt-get install python-rosinstall
-
-# Hector packages
-sudo apt-get install ros-indigo-hector-quadrotor \
-    ros-indigo-octomap-msgs \
-    ros-indigo-hector-gazebo
-

From e60c1a842c856e1a19fcdc1b169dfdbc813e9ce2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 15 Jan 2015 12:37:24 +0100
Subject: [PATCH 292/416] ros mixer: add iris

---
 launch/iris.launch                            |  1 +
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 17 +++++++++++++++--
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/launch/iris.launch b/launch/iris.launch
index 81a137173a..44a0ae0341 100644
--- a/launch/iris.launch
+++ b/launch/iris.launch
@@ -14,6 +14,7 @@
 	<param name="MC_YAWRATE_P" type="double" value="0.5" />
 	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
 	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
+	<param name="mixer" type="string" value="i" />
 </group>
 
 </launch>
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 9954692bc1..54f5fa78b6 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -108,12 +108,19 @@ const MultirotorMixer::Rotor _config_quad_wide[] = {
 	{  0.927184,  0.374607, -1.000000 },
 	{ -0.777146, -0.629320, -1.000000 },
 };
+const MultirotorMixer::Rotor _config_quad_iris[] = {
+	{ -0.876559,  0.481295,  1.000000 },
+	{  0.826590, -0.562805,  1.000000 },
+	{  0.876559,  0.481295, -1.000000 },
+	{ -0.826590, -0.562805, -1.000000 },
+};
 
-const MultirotorMixer::Rotor *_config_index[4] = {
+const MultirotorMixer::Rotor *_config_index[5] = {
 	&_config_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_plus_euroc[0],
-	&_config_quad_wide[0]
+	&_config_quad_wide[0],
+	&_config_quad_iris[0]
 };
 
 MultirotorMixer::MultirotorMixer():
@@ -216,13 +223,19 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro
 	_n.getParamCached("mixer", mixer_name);
 	if (mixer_name == "x") {
 		_rotors = _config_index[0];
+		ROS_WARN("using x");
 	} else if (mixer_name == "+") {
 		_rotors = _config_index[1];
 	} else if (mixer_name == "e") {
 		_rotors = _config_index[2];
 	} else if (mixer_name == "w") {
 		_rotors = _config_index[3];
+		ROS_WARN("using w");
+	} else if (mixer_name == "i") {
+		_rotors = _config_index[4];
+		ROS_WARN("using i");
 	}
+	ROS_WARN("mixer_name %s", mixer_name.c_str());
 
 	// mix
 	mix();

From ac76cdbc373cd09210ad0d35db91ff08c39cf872 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 16 Jan 2015 11:42:00 +0100
Subject: [PATCH 293/416] added catkin_make

---
 Tools/ros/px4_workspace_create.sh | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh
index d8d65c8199..4055f73201 100755
--- a/Tools/ros/px4_workspace_create.sh
+++ b/Tools/ros/px4_workspace_create.sh
@@ -5,3 +5,4 @@ mkdir -p catkin_ws/src
 cd catkin_ws/src
 catkin_init_workspace
 cd ..
+catkin_make

From 18b4e1583663c3066d8fd4496ce74f2f8bf23103 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 16 Jan 2015 12:10:33 +0100
Subject: [PATCH 294/416] added needed dependencies because or recent changes
 in ROS

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 4 ++++
 Tools/ros/px4_workspace_setup.sh         | 8 +++++++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index d83039623e..13532049f7 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -23,3 +23,7 @@ source ~/.bashrc
 
 # get rosinstall
 sudo apt-get install python-rosinstall
+
+# additional dependencies necessary
+sudo apt-get install ros-indigo-octomap-msgs
+
diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
index 233d216718..0a7e1742c4 100755
--- a/Tools/ros/px4_workspace_setup.sh
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -15,9 +15,15 @@ cd euroc_simulator
 git checkout px4_nodes
 cd ..
 
-# # mav comm
+# mav comm
 git clone https://github.com/PX4/mav_comm.git
 
+# glog catkin 
+git clone https://github.com/ethz-asl/glog_catkin.git
+
+# catkin simple
+git clone https://github.com/catkin/catkin_simple.git
+
 cd ..
 
 catkin_make

From dfba2f3cb00e65bb240975e4a9cbfa03e6189e96 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 16 Jan 2015 15:18:53 +0100
Subject: [PATCH 295/416] add drcsim to workspace setup script

---
 Tools/ros/px4_workspace_setup.sh | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
index 0a7e1742c4..7c8af6e8f9 100755
--- a/Tools/ros/px4_workspace_setup.sh
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -18,12 +18,17 @@ cd ..
 # mav comm
 git clone https://github.com/PX4/mav_comm.git
 
-# glog catkin 
+# glog catkin
 git clone https://github.com/ethz-asl/glog_catkin.git
 
 # catkin simple
 git clone https://github.com/catkin/catkin_simple.git
 
+# drcsim (for scenery and models)
+hg clone https://bitbucket.org/osrf/osrf-common
+hg clone https://bitbucket.org/osrf/sandia-hand
+hg clone https://bitbucket.org/osrf/drcsim
+
 cd ..
 
 catkin_make

From 94091a1ce7439ca6744137fae11d07442e8f3622 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 17 Jan 2015 16:46:20 +0100
Subject: [PATCH 296/416] fix dependencies in CMakeLists.txt

---
 CMakeLists.txt | 30 ++++++++++--------------------
 1 file changed, 10 insertions(+), 20 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index eca28b15e6..2de236ff6a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -103,9 +103,8 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS src/include
   LIBRARIES px4
-  CATKIN_DEPENDS roscpp rospy std_msgs
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
   DEPENDS system_lib
-  CATKIN_DEPENDS message_runtime
 )
 
 ###########
@@ -132,6 +131,7 @@ add_library(px4
   src/lib/mathlib/math/Limits.cpp
   src/modules/systemlib/circuit_breaker.cpp
 )
+add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp)
 
 target_link_libraries(px4
   ${catkin_LIBRARIES}
@@ -141,12 +141,7 @@ target_link_libraries(px4
 add_executable(publisher
 	src/examples/publisher/publisher_main.cpp
 	src/examples/publisher/publisher_example.cpp)
-
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-add_dependencies(publisher px4_generate_messages_cpp)
-
-## Specify libraries to link a library or executable target against
+add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp)
 target_link_libraries(publisher
   ${catkin_LIBRARIES}
   px4
@@ -156,12 +151,7 @@ target_link_libraries(publisher
 add_executable(subscriber
 	src/examples/subscriber/subscriber_main.cpp
 	src/examples/subscriber/subscriber_example.cpp)
-
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-add_dependencies(subscriber px4_generate_messages_cpp)
-
-## Specify libraries to link a library or executable target against
+add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp)
 target_link_libraries(subscriber
   ${catkin_LIBRARIES}
   px4
@@ -172,7 +162,7 @@ add_executable(mc_att_control
 	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
 	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
 	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
-add_dependencies(mc_att_control px4_generate_messages_cpp)
+add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(mc_att_control
   ${catkin_LIBRARIES}
   px4
@@ -181,7 +171,7 @@ target_link_libraries(mc_att_control
 ## Attitude Estimator dummy
 add_executable(attitude_estimator
 	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
-add_dependencies(attitude_estimator px4_generate_messages_cpp)
+add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(attitude_estimator
   ${catkin_LIBRARIES}
   px4
@@ -190,7 +180,7 @@ target_link_libraries(attitude_estimator
 ## Manual input
 add_executable(manual_input
 	src/platforms/ros/nodes/manual_input/manual_input.cpp)
-add_dependencies(manual_input px4_generate_messages_cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(manual_input
   ${catkin_LIBRARIES}
   px4
@@ -199,7 +189,7 @@ target_link_libraries(manual_input
 ## Multicopter Mixer dummy
 add_executable(mc_mixer
   src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
-add_dependencies(mc_mixer px4_generate_messages_cpp)
+add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(mc_mixer
   ${catkin_LIBRARIES}
   px4
@@ -208,7 +198,7 @@ target_link_libraries(mc_mixer
 ## Commander
 add_executable(commander
 	src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input px4_generate_messages_cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
 target_link_libraries(commander
   ${catkin_LIBRARIES}
   px4
@@ -229,7 +219,7 @@ target_link_libraries(commander
 # )
 
 ## Mark executables and/or libraries for installation
-install(TARGETS px4 publisher subscriber
+install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

From 93173190fd1f20b9c9a939d423d8cfe6cf13f562 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 16 Jan 2015 18:10:16 +0100
Subject: [PATCH 297/416] removed drcsim, set bash as executing shell

---
 Tools/ros/px4_workspace_setup.sh | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
index 7c8af6e8f9..8c50579b6c 100755
--- a/Tools/ros/px4_workspace_setup.sh
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -1,4 +1,4 @@
-#!/bin/sh
+#!/bin/bash
 # run this script from the root of your catkin_ws
 source devel/setup.bash
 cd src
@@ -27,7 +27,6 @@ git clone https://github.com/catkin/catkin_simple.git
 # drcsim (for scenery and models)
 hg clone https://bitbucket.org/osrf/osrf-common
 hg clone https://bitbucket.org/osrf/sandia-hand
-hg clone https://bitbucket.org/osrf/drcsim
 
 cd ..
 

From 43f3e3ac789d7af15b41ba5f857cc8596b70999d Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 16 Jan 2015 18:21:06 +0100
Subject: [PATCH 298/416] adding drcsim binary installation

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index 13532049f7..80f684590b 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -2,14 +2,17 @@
 
 # main ROS Setup
 # following http://wiki.ros.org/indigo/Installation/Ubuntu
+# also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install
 # run this file with . ./px4_ros_setup_ubuntu.sh
 
 ## add ROS repository
 sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list'
 
 ## add key
 wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \
     sudo apt-key add -
+wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
 
 ## Install main ROS pacakges
 sudo apt-get update
@@ -26,4 +29,5 @@ sudo apt-get install python-rosinstall
 
 # additional dependencies necessary
 sudo apt-get install ros-indigo-octomap-msgs
+sudo apt-get install drcsim
 

From 4c9cc4175b692c1074a64d16ef02b21dc44594d8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 19 Jan 2015 07:47:12 +0100
Subject: [PATCH 299/416] ros: update ubuntu install scripts

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 18 ++++++++++++------
 Tools/ros/px4_workspace_setup.sh         |  2 --
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index 80f684590b..5bca6dfc46 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -2,17 +2,15 @@
 
 # main ROS Setup
 # following http://wiki.ros.org/indigo/Installation/Ubuntu
-# also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install
+# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install
 # run this file with . ./px4_ros_setup_ubuntu.sh
 
 ## add ROS repository
 sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list'
 
 ## add key
 wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \
     sudo apt-key add -
-wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
 
 ## Install main ROS pacakges
 sudo apt-get update
@@ -22,12 +20,20 @@ rosdep update
 
 ## Setup environment variables
 echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
-source ~/.bashrc
+. ~/.bashrc
 
 # get rosinstall
 sudo apt-get install python-rosinstall
 
-# additional dependencies necessary
+# additional dependencies
 sudo apt-get install ros-indigo-octomap-msgs
-sudo apt-get install drcsim
 
+## drcsim setup (for models)
+### add osrf repository
+sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list'
+
+### add key
+wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
+
+### install drcsim
+sudo apt-get install drcsim
diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
index 8c50579b6c..1675e54f3b 100755
--- a/Tools/ros/px4_workspace_setup.sh
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -25,8 +25,6 @@ git clone https://github.com/ethz-asl/glog_catkin.git
 git clone https://github.com/catkin/catkin_simple.git
 
 # drcsim (for scenery and models)
-hg clone https://bitbucket.org/osrf/osrf-common
-hg clone https://bitbucket.org/osrf/sandia-hand
 
 cd ..
 

From a5caf1c99be599401379743b675862b0601fed6e Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 19 Jan 2015 10:17:58 +0100
Subject: [PATCH 300/416] adding '-y' option to apt-get and missing update
 before drcsim installation

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index 5bca6dfc46..72b4f94681 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -14,7 +14,7 @@ wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \
 
 ## Install main ROS pacakges
 sudo apt-get update
-sudo apt-get install ros-indigo-desktop-full
+sudo apt-get -y install ros-indigo-desktop-full
 sudo rosdep init
 rosdep update
 
@@ -23,10 +23,10 @@ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
 . ~/.bashrc
 
 # get rosinstall
-sudo apt-get install python-rosinstall
+sudo apt-get -y install python-rosinstall
 
 # additional dependencies
-sudo apt-get install ros-indigo-octomap-msgs
+sudo apt-get -y install ros-indigo-octomap-msgs
 
 ## drcsim setup (for models)
 ### add osrf repository
@@ -36,4 +36,5 @@ sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main"
 wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add -
 
 ### install drcsim
-sudo apt-get install drcsim
+sudo apt-get update
+sudo apt-get -y install drcsim

From f176147d2af34d83d2e0c27cae8b98dfe995d812 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Jan 2015 11:04:56 +0100
Subject: [PATCH 301/416] Allow GCC 4.9.3

---
 makefiles/toolchain_gnu-arm-eabi.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index ef70d51f20..3969804535 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -50,7 +50,7 @@ OBJDUMP			 = $(CROSSDEV)objdump
 
 # Check if the right version of the toolchain is available
 #
-CROSSDEV_VER_SUPPORTED	 = 4.7.4 4.7.5 4.7.6 4.8.4
+CROSSDEV_VER_SUPPORTED	 = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3
 CROSSDEV_VER_FOUND	 = $(shell $(CC) -dumpversion)
 
 ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))

From 6574692bd7a4101113cf6a80bcfae968d5a6e4f1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Jan 2015 11:01:25 +0100
Subject: [PATCH 302/416] uORB: Remove unused function

---
 src/modules/uORB/uORB.cpp | 12 ------------
 1 file changed, 12 deletions(-)

diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 149b8f6d24..2fbb05fee7 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[])
 namespace
 {
 
-void debug(const char *fmt, ...)
-{
-	va_list ap;
-
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, "\n");
-	fflush(stderr);
-	usleep(100000);
-}
-
 /**
  * Advertise a node; don't consider it an error if the node has
  * already been advertised.

From fd275147e4ae2efe2803e0bdc8f2c2929db70b5d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Jan 2015 13:39:13 +0100
Subject: [PATCH 303/416] Add hackery on NuttX header, to be removed during
 rebase -i

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index dbcccb2455..3d8171f6ea 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83
+Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0

From 85b6907e1db1a6af88fe469e8e08dbd0a9d7a2a7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Jan 2015 13:39:36 +0100
Subject: [PATCH 304/416] Fixes to make GCC 4.9 link

---
 makefiles/toolchain_gnu-arm-eabi.mk | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 3969804535..a8b4f18110 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -110,9 +110,7 @@ ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
 			   -fno-strength-reduce \
 			   -fomit-frame-pointer \
 			   -funsafe-math-optimizations \
-			   -fno-builtin-printf \
-			   -ffunction-sections \
-			   -fdata-sections
+			   -fno-builtin-printf
 
 # enable precise stack overflow tracking
 # note - requires corresponding support in NuttX
@@ -166,7 +164,8 @@ ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
-EXTRA_LIBS		+= $(LIBM)
+LIBC			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libc.a)
+EXTRA_LIBS		+= $(LIBM) $(LIBC)
 
 # Flags we pass to the C compiler
 #

From d1eac3510bdee3aa133a9eea248f882758dd29d0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Jan 2015 13:40:10 +0100
Subject: [PATCH 305/416] DELETE DURING REBASE: Remove some apps to make space

---
 makefiles/config_px4fmu-v2_default.mk | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a5f996bb35..2ba07821fa 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -31,7 +31,7 @@ MODULES		+= drivers/mb12xx
 MODULES		+= drivers/ll40ls
 # MODULES		+= drivers/trone
 MODULES		+= drivers/gps
-MODULES		+= drivers/hil
+# MODULES		+= drivers/hil
 # MODULES		+= drivers/hott
 # MODULES		+= drivers/hott/hott_telemetry
 # MODULES		+= drivers/hott/hott_sensors
@@ -76,7 +76,7 @@ MODULES		+= modules/uavcan
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/ekf_att_pos_estimator
+# MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 
 #

From 7c3223b8609ee418b520d19cae7e52d2a7a85e99 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 18 Jan 2015 18:43:45 +0100
Subject: [PATCH 306/416] added a messageplayer prototype for ros

---
 CMakeLists.txt                                |  85 +++++----
 makefiles/config_px4fmu-v2_multiplatform.mk   |   3 +-
 makefiles/config_px4fmu-v2_test.mk            |   2 +-
 msg/templates/msg.h.template                  |  74 +++++--
 src/examples/publisher/publisher_example.cpp  |  19 +-
 src/examples/publisher/publisher_example.h    |   1 +
 .../subscriber/subscriber_example.cpp         |  14 +-
 src/examples/subscriber/subscriber_example.h  |   3 +-
 .../nuttx/px4_messages/px4_rc_channels.h      |  26 +++
 src/platforms/px4_includes.h                  |   4 +
 src/platforms/px4_message.h                   |  77 ++++++++
 src/platforms/px4_nodehandle.h                | 180 ++++++++++++------
 src/platforms/px4_publisher.h                 |  41 +++-
 src/platforms/px4_subscriber.h                | 114 +++++++----
 .../ros/px4_messages/px4_rc_channels.h        |  25 +++
 15 files changed, 496 insertions(+), 172 deletions(-)
 create mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h
 create mode 100644 src/platforms/px4_message.h
 create mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2de236ff6a..0ac9912df3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -116,6 +116,7 @@ catkin_package(
 include_directories(
   ${catkin_INCLUDE_DIRS}
   src/platforms
+  src/platforms/ros/px4_messages
   src/include
   src/modules
   src/
@@ -157,52 +158,52 @@ target_link_libraries(subscriber
   px4
 )
 
-## MC Attitude Control
-add_executable(mc_att_control
-	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
-	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
-	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
-add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_att_control
-  ${catkin_LIBRARIES}
-  px4
-)
+# ## MC Attitude Control
+# add_executable(mc_att_control
+	# src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+	# src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+	# src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
+# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(mc_att_control
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
-## Attitude Estimator dummy
-add_executable(attitude_estimator
-	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
-add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(attitude_estimator
-  ${catkin_LIBRARIES}
-  px4
-)
+# ## Attitude Estimator dummy
+# add_executable(attitude_estimator
+	# src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(attitude_estimator
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
-## Manual input
-add_executable(manual_input
-	src/platforms/ros/nodes/manual_input/manual_input.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(manual_input
-  ${catkin_LIBRARIES}
-  px4
-)
+# ## Manual input
+# add_executable(manual_input
+	# src/platforms/ros/nodes/manual_input/manual_input.cpp)
+# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(manual_input
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
-## Multicopter Mixer dummy
-add_executable(mc_mixer
-  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
-add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_mixer
-  ${catkin_LIBRARIES}
-  px4
-)
+# ## Multicopter Mixer dummy
+# add_executable(mc_mixer
+  # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
+# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(mc_mixer
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
-## Commander
-add_executable(commander
-	src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(commander
-  ${catkin_LIBRARIES}
-  px4
-)
+# ## Commander
+# add_executable(commander
+	# src/platforms/ros/nodes/commander/commander.cpp)
+# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(commander
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
 #############
 ## Install ##
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index bed2fa5061..bfa26f83cf 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -86,7 +86,8 @@ MODULES		+= modules/position_estimator_inav
 #MODULES		+= modules/fw_pos_control_l1
 #MODULES		+= modules/fw_att_control
 # MODULES		+= modules/mc_att_control
-MODULES		+= modules/mc_att_control_multiplatform
+# MODULES		+= modules/mc_att_control_multiplatform
+MODULES		+= examples/publisher
 MODULES		+= modules/mc_pos_control
 MODULES 	+= modules/vtol_att_control
 
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index fa2c832625..4defe49e75 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -56,7 +56,7 @@ MODULES		+= systemcmds/ver
 # Example modules
 #
 MODULES		+= examples/matlab_csv_serial
-MODULES		+= examples/subscriber
+#MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
 
 #
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index 19068a3a1e..c27daed835 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -95,17 +95,17 @@ for field in spec.parsed_fields():
 @##############################
 @{
 
-type_map = {'int8': 'int8_t',
-  'int16': 'int16_t',
-  'int32': 'int32_t',
-  'int64': 'int64_t',
-  'uint8': 'uint8_t',
-  'uint16': 'uint16_t',
-  'uint32': 'uint32_t',
-  'uint64': 'uint64_t',
-  'float32': 'float',
-  'bool': 'bool',
-  'fence_vertex': 'fence_vertex'}
+type_map = {'int8': ['int8_t', '0'],
+  'int16': ['int16_t', '0'],
+  'int32': ['int32_t', '0'],
+  'int64': ['int64_t', '0'],
+  'uint8': ['uint8_t', '0'],
+  'uint16': ['uint16_t', '0'],
+  'uint32': ['uint32_t', '0'],
+  'uint64': ['uint64_t', '0'],
+  'float32': ['float', '0.0f'],
+  'bool': ['bool', 'false'],
+  'fence_vertex': ['fence_vertex', '']}
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -129,20 +129,70 @@ def print_field_def(field):
 
   if type in type_map:
     # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type]
+    type_px4 = type_map[type][0]
   else:
     raise Exception("Type {0} not supported, add to to template file!".format(type))
 
   print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
 
+# Function to init fields
+def get_field_init(field):
+  type = field.type
+  # detect embedded types
+  sl_pos = type.find('/')
+  type_appendix = ''
+  type_prefix = ''
+  if (sl_pos >= 0):
+    type = type[sl_pos + 1:]
+    type_prefix = 'struct '
+    type_appendix = '_s'
+
+  # detect arrays
+  a_pos = type.find('[')
+  array_size = ''
+  if (a_pos >= 0):
+    # field is array
+    array_size = type[a_pos:]
+    type = type[:a_pos]
+    return '\n\t%s{},'%(field.name)
+
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type][0]
+    init_value = type_map[type][1]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  return '\n\t%s(%s),'%(field.name, init_value)
 }
+@#ifdef __cplusplus
+@#class @(spec.short_name)_s {
+@#public:
+@#else
 struct @(spec.short_name)_s {
+@#endif
 @{
 # loop over all fields and print the type and name
 for field in spec.parsed_fields():
   if (not field.is_header):
     print_field_def(field)
 }@
+
+@##ifdef __cplusplus
+@#@(spec.short_name)_s() :
+@#@{
+@#field_init = ''
+@## loop over all fields and init
+@#for field in spec.parsed_fields():
+@#  if (not field.is_header):
+@#    field_init += get_field_init(field)
+@#
+@#print(field_init[:-1])
+@#}@
+@#{}
+@#virtual ~@(spec.short_name)_s() {}
+@##endif
+
 };
 
 /**
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index 2e5779ebec..e85e42b386 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -45,7 +45,8 @@ using namespace px4;
 
 PublisherExample::PublisherExample() :
 	_n(),
-	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
+	_rc_channels_pub(_n.advertise<px4_rc_channels>())
+	// _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
 {
 
 }
@@ -55,11 +56,19 @@ int PublisherExample::main()
 	px4::Rate loop_rate(10);
 
 	while (px4::ok()) {
-		PX4_TOPIC_T(rc_channels) msg;
-		msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.timestamp_last_valid);
+		// PX4_TOPIC_T(rc_channels) msg;
+		// msg.timestamp_last_valid = px4::get_time_micros();
+		// PX4_INFO("%llu", msg.timestamp_last_valid);
+
+		// _rc_channels_pub->publish(msg);
+
+		//XXX
+		px4_rc_channels msg2;
+		msg2.data().timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("%llu", msg2.data().timestamp_last_valid);
+		// msg2.pub->publish2();
+		_rc_channels_pub->publish(msg2);
 
-		_rc_channels_pub->publish(msg);
 
 		_n.spinOnce();
 		loop_rate.sleep();
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 304ecef47a..86586d0d34 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -48,5 +48,6 @@ public:
 	int main();
 protected:
 	px4::NodeHandle _n;
+	// px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
 	px4::Publisher * _rc_channels_pub;
 };
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 3c80561caf..1c8f4c62bf 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -62,11 +62,13 @@ SubscriberExample::SubscriberExample() :
 
 	/* Do some subscriptions */
 	/* Function */
-	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
-	/* Class Method */
-	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
-	/* No callback */
-	_sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function);
+
+	// [> Class Method <]
+	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+	// [> No callback <]
+	// _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
 
 	PX4_INFO("subscribed");
 }
@@ -78,5 +80,5 @@ SubscriberExample::SubscriberExample() :
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
 			msg.timestamp_last_valid,
-			_sub_rc_chan->get().timestamp_last_valid);
+			_sub_rc_chan->get().data().timestamp_last_valid);
 }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index eb2c956e09..b52ae991bf 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -56,7 +56,8 @@ protected:
 	int32_t _interval;
 	px4_param_t _p_test_float;
 	float _test_float;
-	px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+	// px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
 	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 
diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
new file mode 100644
index 0000000000..bfca484698
--- /dev/null
+++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
@@ -0,0 +1,26 @@
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include "platforms/px4_message.h"
+
+#pragma once
+namespace px4
+{
+
+class  px4_rc_channels :
+	public PX4Message<rc_channels_s>
+{
+public:
+	px4_rc_channels() :
+		PX4Message<rc_channels_s>()
+	{}
+
+	px4_rc_channels(rc_channels_s msg) :
+		PX4Message<rc_channels_s>(msg)
+	{}
+
+	~px4_rc_channels() {}
+
+	PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);}
+};
+
+}
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index fc31162c8a..bdb2b8de9b 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -49,6 +49,7 @@
 #ifdef __cplusplus
 #include "ros/ros.h"
 #include "px4/rc_channels.h"
+#include "px4_rc_channels.h" //XXX
 #include "px4/vehicle_attitude.h"
 #include <px4/vehicle_attitude_setpoint.h>
 #include <px4/manual_control_setpoint.h>
@@ -71,6 +72,9 @@
 #include <nuttx/config.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
+#ifdef __cplusplus
+#include <platforms/nuttx/px4_messages/px4_rc_channels.h> //XXX
+#endif
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h
new file mode 100644
index 0000000000..05fcf1140b
--- /dev/null
+++ b/src/platforms/px4_message.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_message.h
+ *
+ * Defines the message base types
+ */
+#pragma once
+
+#if defined(__PX4_ROS)
+typedef const char* PX4TopicHandle;
+#else
+#include <uORB/uORB.h>
+typedef const struct orb_metatdata* PX4TopicHandle;
+#endif
+
+namespace px4
+{
+
+template <typename M>
+class PX4Message
+{
+	// friend class NodeHandle;
+// #if defined(__PX4_ROS)
+	// template<typename T>
+	// friend class SubscriberROS;
+// #endif
+
+public:
+	PX4Message() :
+		_data()
+	{}
+
+	PX4Message(M msg) :
+		_data(msg)
+	{}
+
+	virtual ~PX4Message() {};
+
+	virtual M& data() {return _data;}
+	virtual PX4TopicHandle handle() = 0;
+private:
+	M _data;
+};
+
+}
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 7b14caed97..a405812394 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -48,6 +48,7 @@
 #include "ros/ros.h"
 #include <list>
 #include <inttypes.h>
+#include <type_traits>
 #else
 /* includes when building for NuttX */
 #include <poll.h>
@@ -77,15 +78,25 @@ public:
 	 * @param topic		Name of the topic
 	 * @param fb		Callback, executed on receiving a new message
 	 */
-	template<typename M>
-	Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
+	// template<typename M>
+	// Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
+	// {
+		// SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
+		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+					  // (SubscriberROS<M> *)sub);
+		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+		// _subs.push_back(sub);
+		// return (Subscriber<M> *)sub;
+	// }
+	template<typename T>
+	Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
 	{
-		SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  (SubscriberROS<M> *)sub);
-		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
+				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
+		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
-		return (Subscriber<M> *)sub;
+		return (Subscriber<T> *)sub;
 	}
 
 	/**
@@ -93,40 +104,49 @@ public:
 	 * @param topic		Name of the topic
 	 * @param fb		Callback, executed on receiving a new message
 	 */
-	template<typename M, typename T>
-	Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
-	{
-		SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  (SubscriberROS<M> *)sub);
-		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		_subs.push_back(sub);
-		return (Subscriber<M> *)sub;
-	}
+	// template<typename M, typename T>
+	// Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
+	// {
+		// SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
+		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+					  // (SubscriberROS<M> *)sub);
+		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+		// _subs.push_back(sub);
+		// return (Subscriber<M> *)sub;
+	// }
 
 	/**
 	 * Subscribe with no callback, just the latest value is stored on updates
 	 * @param topic		Name of the topic
 	 */
-	template<typename M>
-	Subscriber<M> *subscribe(const char *topic)
-	{
-		SubscriberBase *sub = new SubscriberROS<M>();
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  (SubscriberROS<M> *)sub);
-		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		_subs.push_back(sub);
-		return (Subscriber<M> *)sub;
-	}
+	// template<typename M>
+	// Subscriber<M> *subscribe(const char *topic)
+	// {
+		// SubscriberBase *sub = new SubscriberROS<M>();
+		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+					  // (SubscriberROS<M> *)sub);
+		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+		// _subs.push_back(sub);
+		// return (Subscriber<M> *)sub;
+	// }
 
 	/**
 	 * Advertise topic
 	 * @param topic		Name of the topic
 	 */
-	template<typename M>
-	Publisher *advertise(const char *topic)
+	// template<typename M>
+	// Publisher *advertise(const char *topic)
+	// {
+		// ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
+		// Publisher *pub =  new Publisher(ros_pub);
+		// _pubs.push_back(pub);
+		// return pub;
+	// }
+	template<typename T>
+	Publisher* advertise()
+	// Publisher<T> *advertise()
 	{
-		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
+		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>((new T())->handle(), kQueueSizeDefault);
 		Publisher *pub =  new Publisher(ros_pub);
 		_pubs.push_back(pub);
 		return pub;
@@ -161,7 +181,7 @@ public:
 	~NodeHandle()
 	{
 		/* Empty subscriptions list */
-		uORB::SubscriptionNode *sub = _subs.getHead();
+		SubscriberNode *sub = _subs.getHead();
 		int count = 0;
 
 		while (sub != nullptr) {
@@ -170,13 +190,13 @@ public:
 				break;
 			}
 
-			uORB::SubscriptionNode *sib = sub->getSibling();
+			SubscriberNode *sib = sub->getSibling();
 			delete sub;
 			sub = sib;
 		}
 
 		/* Empty publications list */
-		uORB::PublicationNode *pub = _pubs.getHead();
+		Publisher *pub = _pubs.getHead();
 		count = 0;
 
 		while (pub != nullptr) {
@@ -185,7 +205,7 @@ public:
 				break;
 			}
 
-			uORB::PublicationNode *sib = pub->getSibling();
+			Publisher *sib = pub->getSibling();
 			delete pub;
 			pub = sib;
 		}
@@ -198,19 +218,41 @@ public:
 	 * @param interval	Minimal interval between calls to callback
 	 */
 
-	template<typename M>
-	Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 std::function<void(const M &)> callback,
-				 unsigned interval)
+	// template<typename M>
+	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
+				 // std::function<void(const M &)> callback,
+				 // unsigned interval)
+	// {
+		// SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
+
+		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
+		// if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
+			// _sub_min_interval = sub_px4;
+		// }
+
+		// return (Subscriber<M> *)sub_px4;
+	// }
+	/**
+	 * Subscribe with callback to function
+	 * @param meta		Describes the topic which nodehande should subscribe to
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 */
+
+	template<typename T>
+	Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
 	{
-		SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
+		const struct orb_metadata * meta = NULL;
+		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval);
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback);
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
 			_sub_min_interval = sub_px4;
 		}
+		_subs.add((SubscriberNode *)sub_px4);
 
-		return (Subscriber<M> *)sub_px4;
+		return (Subscriber<T> *)sub_px4;
 	}
 
 	/**
@@ -219,29 +261,47 @@ public:
 	 * @param interval	Minimal interval between data fetches from orb
 	 */
 
-	template<typename M>
-	Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 unsigned interval)
-	{
-		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
+	// template<typename M>
+	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
+				 // unsigned interval)
+	// {
+		// SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
 
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
-			_sub_min_interval = sub_px4;
-		}
+		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
+		// if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
+			// _sub_min_interval = sub_px4;
+		// }
 
-		return (Subscriber<M> *)sub_px4;
-	}
+		// return (Subscriber<M> *)sub_px4;
+	// }
 
 	/**
 	 * Advertise topic
 	 * @param meta		Describes the topic which is advertised
 	 */
-	template<typename M>
-	Publisher *advertise(const struct orb_metadata *meta)
+	// template<typename M>
+	// Publisher *advertise(const struct orb_metadata *meta)
+	// {
+		// //XXX
+		// Publisher *pub = new Publisher(meta, &_pubs);
+		// return pub;
+	// }
+
+	/**
+	 * Advertise topic
+	 * @param meta		Describes the topic which is advertised
+	 */
+	template<typename T>
+	Publisher *advertise()
 	{
 		//XXX
-		Publisher *pub = new Publisher(meta, &_pubs);
+		// uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle());
+		const struct orb_metadata * meta = NULL;
+		uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta);
+		Publisher *pub = new Publisher(uorb_pub);
+
+		_pubs.add(pub);
+
 		return pub;
 	}
 
@@ -251,7 +311,7 @@ public:
 	void spinOnce()
 	{
 		/* Loop through subscriptions, call callback for updated subscriptions */
-		uORB::SubscriptionNode *sub = _subs.getHead();
+		SubscriberNode *sub = _subs.getHead();
 		int count = 0;
 
 		while (sub != nullptr) {
@@ -281,7 +341,7 @@ public:
 
 			/* Poll fd with smallest interval */
 			struct pollfd pfd;
-			pfd.fd = _sub_min_interval->getHandle();
+			pfd.fd = _sub_min_interval->getUORBHandle();
 			pfd.events = POLLIN;
 			poll(&pfd, 1, timeout_ms);
 			spinOnce();
@@ -290,9 +350,9 @@ public:
 private:
 	static const uint16_t kMaxSubscriptions = 100;
 	static const uint16_t kMaxPublications = 100;
-	List<uORB::SubscriptionNode *> _subs;		/**< Subcriptions of node */
-	List<uORB::PublicationNode *> _pubs;		/**< Publications of node */
-	uORB::SubscriptionNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
+	List<SubscriberNode *> _subs;		/**< Subcriptions of node */
+	List<Publisher *> _pubs;		/**< Publications of node */
+	SubscriberNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };
 #endif
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index c6f3d6108e..aff045d23b 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -46,6 +46,8 @@
 #include <containers/List.hpp>
 #endif
 
+#include <platforms/px4_message.h>
+
 namespace px4
 {
 
@@ -58,11 +60,19 @@ public:
 	PublisherBase() {};
 	~PublisherBase() {};
 
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
+	// virtual int publish2(const PX4Message * const msg) = 0;
+
 };
 
 #if defined(__PX4_ROS)
+// template <typename T>
 class Publisher :
 	public PublisherBase
+	// public PublisherBase,
+	// public T
 {
 public:
 	/**
@@ -71,6 +81,7 @@ public:
 	 */
 	Publisher(ros::Publisher ros_pub) :
 		PublisherBase(),
+		// T(),
 		_ros_pub(ros_pub)
 	{}
 
@@ -79,15 +90,20 @@ public:
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic
 	 */
-	template<typename M>
-	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+	// int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+	template <typename T>
+	int publish(T &msg) {
+		_ros_pub.publish(msg.data());
+		return 0;}
 private:
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
 class Publisher :
-	public uORB::PublicationNode,
-	public PublisherBase
+	// public uORB::PublicationNode,
+	public PublisherBase,
+	public ListNode<Publisher *>
+
 {
 public:
 	/**
@@ -95,10 +111,14 @@ public:
 	 * @param meta	    orb metadata for the topic which is used
 	 * @param list	    publisher is added to this list
 	 */
-	Publisher(const struct orb_metadata *meta,
-		  List<uORB::PublicationNode *> *list) :
-		uORB::PublicationNode(meta, list),
-		PublisherBase()
+	// Publisher(const struct orb_metadata *meta,
+		  // List<uORB::PublicationNode *> *list) :
+		// uORB::PublicationNode(meta, list),
+		// PublisherBase()
+	// {}
+	Publisher(uORB::PublicationBase * uorb_pub) :
+		PublisherBase(),
+		_uorb_pub(uorb_pub)
 	{}
 
 	~Publisher() {};
@@ -109,7 +129,7 @@ public:
 	template<typename M>
 	int publish(const M &msg)
 	{
-		uORB::PublicationBase::update((void *)&msg);
+		_uorb_pub->update((void *)&msg);
 		return 0;
 	}
 
@@ -117,6 +137,9 @@ public:
 	 * Empty callback for list traversal
 	 */
 	void update() {} ;
+private:
+	uORB::PublicationBase * _uorb_pub;	/**< Handle to the publisher */
+
 };
 #endif
 }
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index aaacf9e484..ef03922ad3 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -39,6 +39,7 @@
 #pragma once
 
 #include <functional>
+#include <type_traits>
 
 #if defined(__PX4_ROS)
 /* includes when building for ros */
@@ -67,7 +68,7 @@ public:
 /**
  * Subscriber class which is used by nodehandle
  */
-template<typename M>
+template<typename T>
 class Subscriber :
 	public SubscriberBase
 {
@@ -81,7 +82,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	virtual M get() = 0;
+	virtual T get() = 0;
 
 	/**
 	 * Get void pointer to last message value
@@ -93,9 +94,9 @@ public:
 /**
  * Subscriber class that is templated with the ros n message type
  */
-template<typename M>
+template<typename T>
 class SubscriberROS :
-	public Subscriber<M>
+	public Subscriber<T>
 {
 	friend class NodeHandle;
 
@@ -103,8 +104,8 @@ public:
 	/**
 	 * Construct Subscriber by providing a callback function
 	 */
-	SubscriberROS(std::function<void(const M &)> cbf) :
-		Subscriber<M>(),
+	SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+		Subscriber<T>(),
 		_ros_sub(),
 		_cbf(cbf),
 		_msg_current()
@@ -114,7 +115,7 @@ public:
 	 * Construct Subscriber without a callback function
 	 */
 	SubscriberROS() :
-		Subscriber<M>(),
+		Subscriber<T>(),
 		_ros_sub(),
 		_cbf(NULL),
 		_msg_current()
@@ -127,7 +128,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	M get() { return _msg_current; }
+	T get() { return _msg_current; }
 	/**
 	 * Get void pointer to last message value
 	 */
@@ -137,10 +138,10 @@ protected:
 	/**
 	 * Called on topic update, saves the current message and then calls the provided callback function
 	 */
-	void callback(const M &msg)
+	void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
 	{
 		/* Store data */
-		_msg_current = msg;
+		_msg_current = (T)msg;
 
 		/* Call callback */
 		if (_cbf != NULL) {
@@ -158,20 +159,47 @@ protected:
 	}
 
 	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
-	std::function<void(const M &)> _cbf;	/**< Callback that the user provided on the subscription */
-	M _msg_current;				/**< Current Message value */
+	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf;	/**< Callback that the user provided on the subscription */
+	T _msg_current;				/**< Current Message value */
 
 };
 
 #else // Building for NuttX
 
+
+/**
+ * Because we maintain a list of subscribers we need a node class
+ */
+class SubscriberNode :
+	public ListNode<SubscriberNode *>
+{
+public:
+	SubscriberNode(unsigned interval) :
+		ListNode(),
+		_interval(interval)
+	{}
+
+	virtual ~SubscriberNode() {}
+
+	virtual void update() = 0;
+
+	virtual int getUORBHandle() = 0;
+
+	unsigned get_interval() { return _interval; }
+
+protected:
+	unsigned _interval;
+
+};
+
+
 /**
  * Subscriber class that is templated with the uorb subscription message type
  */
-template<typename M>
+template<typename T>
 class SubscriberUORB :
-	public Subscriber<M>,
-	public uORB::Subscription<M>
+	public Subscriber<T>,
+	public SubscriberNode
 {
 public:
 
@@ -181,11 +209,15 @@ public:
 	 * @param interval	Minimal interval between calls to callback
 	 * @param list	    subscriber is added to this list
 	 */
-	SubscriberUORB(const struct orb_metadata *meta,
-		       unsigned interval,
-		       List<uORB::SubscriptionNode *> *list) :
-		Subscriber<M>(),
-		uORB::Subscription<M>(meta, interval, list)
+	// SubscriberUORB(const struct orb_metadata *meta,
+		       // unsigned interval,
+		       // List<uORB::SubscriptionNode *> *list) :
+		// Subscriber<M>(),
+		// uORB::Subscription<M>(meta, interval, list)
+	// {}
+	SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) :
+		SubscriberNode(interval),
+		_uorb_sub(uorb_sub)
 	{}
 
 	~SubscriberUORB() {};
@@ -196,29 +228,36 @@ public:
 	 */
 	virtual void update()
 	{
-		if (!uORB::Subscription<M>::updated()) {
+		if (!_uorb_sub->updated()) {
 			/* Topic not updated */
 			return;
 		}
 
 		/* get latest data */
-		uORB::Subscription<M>::update();
+		_uorb_sub->update(get_void_ptr());
 	};
 
 	/* Accessors*/
 	/**
 	 * Get the last message value
 	 */
-	M get() { return uORB::Subscription<M>::getData(); }
+	T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
 	/**
 	 * Get void pointer to last message value
 	 */
-	void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); }
+	void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; }
+
+	int getUORBHandle() { return _uorb_sub->getHandle(); }
+
+protected:
+	uORB::SubscriptionBase * _uorb_sub;	/**< Handle to the subscription */
+	typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() { return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
 };
 
-template<typename M>
+//XXX reduce to one class with overloaded constructor?
+template<typename T>
 class SubscriberUORBCallback :
-	public SubscriberUORB<M>
+	public SubscriberUORB<T>
 {
 public:
 	/**
@@ -228,11 +267,16 @@ public:
 	 * @param interval	Minimal interval between calls to callback
 	 * @param list	    subscriber is added to this list
 	 */
-	SubscriberUORBCallback(const struct orb_metadata *meta,
-			       unsigned interval,
-			       std::function<void(const M &)> callback,
-			       List<uORB::SubscriptionNode *> *list) :
-		SubscriberUORB<M>(meta, interval, list),
+	// SubscriberUORBCallback(const struct orb_metadata *meta,
+			       // unsigned interval,
+			       // std::function<void(const M &)> callback,
+			       // List<uORB::SubscriptionNode *> *list) :
+		// SubscriberUORB<M>(meta, interval, list),
+		// _callback(callback)
+	// {}
+	SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
+			       std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) :
+		SubscriberUORB<T>(uorb_sub),
 		_callback(callback)
 	{}
 
@@ -245,13 +289,13 @@ public:
 	 */
 	virtual void update()
 	{
-		if (!uORB::Subscription<M>::updated()) {
+		if (!SubscriberUORB<T>::_uorb_sub->updated()) {
 			/* Topic not updated, do not call callback */
 			return;
 		}
 
 		/* get latest data */
-		uORB::Subscription<M>::update();
+		SubscriberUORB<T>::_uorb_sub->update();
 
 
 		/* Check if there is a callback */
@@ -260,12 +304,12 @@ public:
 		}
 
 		/* Call callback which performs actions based on this data */
-		_callback(uORB::Subscription<M>::getData());
+		_callback(SubscriberUORB<T>::getUORBData());
 
 	};
 
 protected:
-	std::function<void(const M &)> _callback;	/**< Callback handle,
+	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	/**< Callback handle,
 							  called when new data is available */
 };
 #endif
diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h
new file mode 100644
index 0000000000..a5a5ee2020
--- /dev/null
+++ b/src/platforms/ros/px4_messages/px4_rc_channels.h
@@ -0,0 +1,25 @@
+#include "px4/rc_channels.h"
+#include "platforms/px4_message.h"
+
+#pragma once
+namespace px4
+{
+
+class  px4_rc_channels :
+	public PX4Message<rc_channels>
+{
+public:
+	px4_rc_channels() :
+		PX4Message<rc_channels>()
+	{}
+
+	px4_rc_channels(rc_channels msg) :
+		PX4Message<rc_channels>(msg)
+	{}
+
+	~px4_rc_channels() {}
+
+	PX4TopicHandle handle() {return "rc_channels";}
+};
+
+}

From cadcad6ffbdfbe9b92a5936f4d894138f912b4ff Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 20 Jan 2015 18:27:05 +0100
Subject: [PATCH 307/416] messagelayer prototype for nuttx

---
 makefiles/config_px4fmu-v2_test.mk            |  2 +-
 msg/templates/msg.h.template                  |  7 +-
 .../subscriber/subscriber_example.cpp         | 19 ++--
 src/examples/subscriber/subscriber_example.h  |  2 +-
 .../nuttx/px4_messages/px4_rc_channels.h      |  2 +-
 src/platforms/px4_message.h                   |  3 +-
 src/platforms/px4_nodehandle.h                | 20 ++--
 src/platforms/px4_publisher.h                 |  4 +-
 src/platforms/px4_subscriber.h                | 93 +++++++++----------
 9 files changed, 78 insertions(+), 74 deletions(-)

diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 4defe49e75..fa2c832625 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -56,7 +56,7 @@ MODULES		+= systemcmds/ver
 # Example modules
 #
 MODULES		+= examples/matlab_csv_serial
-#MODULES		+= examples/subscriber
+MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
 
 #
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index c27daed835..826418d118 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -165,12 +165,13 @@ def get_field_init(field):
 
   return '\n\t%s(%s),'%(field.name, init_value)
 }
-@#ifdef __cplusplus
+#ifdef __cplusplus
 @#class @(spec.short_name)_s {
+struct __EXPORT @(spec.short_name)_s {
 @#public:
-@#else
+#else
 struct @(spec.short_name)_s {
-@#endif
+#endif
 @{
 # loop over all fields and print the type and name
 for field in spec.parsed_fields():
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 1c8f4c62bf..c662a07ad2 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -43,8 +43,10 @@
 
 using namespace px4;
 
-void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
+// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
+void rc_channels_callback_function(const px4_rc_channels &msg);
+void rc_channels_callback_function(const px4_rc_channels &msg) {
+	PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
 }
 
 SubscriberExample::SubscriberExample() :
@@ -63,7 +65,8 @@ SubscriberExample::SubscriberExample() :
 	/* Do some subscriptions */
 	/* Function */
 	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
-	_n.subscribe<px4_rc_channels>(rc_channels_callback_function);
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version
+	// _n.subscribe<px4_rc_channels>(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version
 
 	// [> Class Method <]
 	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
@@ -77,8 +80,8 @@ SubscriberExample::SubscriberExample() :
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  * Also the current value of the _sub_rc_chan subscription is printed
  */
-void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			msg.timestamp_last_valid,
-			_sub_rc_chan->get().data().timestamp_last_valid);
-}
+// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
+	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			// msg.timestamp_last_valid,
+			// _sub_rc_chan->get().data().timestamp_last_valid);
+// }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index b52ae991bf..43dafe010a 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -59,7 +59,7 @@ protected:
 	// px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
 	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
-	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
+	// void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 
 
 };
diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
index bfca484698..cb1135eb98 100644
--- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h
+++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
@@ -6,7 +6,7 @@
 namespace px4
 {
 
-class  px4_rc_channels :
+class __EXPORT px4_rc_channels :
 	public PX4Message<rc_channels_s>
 {
 public:
diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h
index 05fcf1140b..c908f6fc69 100644
--- a/src/platforms/px4_message.h
+++ b/src/platforms/px4_message.h
@@ -49,7 +49,7 @@ namespace px4
 {
 
 template <typename M>
-class PX4Message
+class __EXPORT PX4Message
 {
 	// friend class NodeHandle;
 // #if defined(__PX4_ROS)
@@ -69,6 +69,7 @@ public:
 	virtual ~PX4Message() {};
 
 	virtual M& data() {return _data;}
+	virtual const M& data() const {return _data;}
 	virtual PX4TopicHandle handle() = 0;
 private:
 	M _data;
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index a405812394..2406a4a775 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -89,7 +89,8 @@ public:
 		// return (Subscriber<M> *)sub;
 	// }
 	template<typename T>
-	Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
+	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
+	Subscriber<T> *subscribe(void(*fp)(const T &))
 	{
 		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
@@ -240,17 +241,20 @@ public:
 	 */
 
 	template<typename T>
-	Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
+	// Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
+	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &),  unsigned interval=10) //XXX interval
+	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
 		const struct orb_metadata * meta = NULL;
 		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval);
-		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback);
+		// SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, callback);
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
 
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
-			_sub_min_interval = sub_px4;
-		}
-		_subs.add((SubscriberNode *)sub_px4);
+		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
+		// if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			// _sub_min_interval = sub_px4;
+		// }
+		// _subs.add((SubscriberNode *)sub_px4);
 
 		return (Subscriber<T> *)sub_px4;
 	}
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index aff045d23b..6d75e28fcc 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -54,7 +54,7 @@ namespace px4
 /**
  * Untemplated publisher base class
  * */
-class PublisherBase
+class __EXPORT PublisherBase
 {
 public:
 	PublisherBase() {};
@@ -99,7 +99,7 @@ private:
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
-class Publisher :
+class __EXPORT Publisher :
 	// public uORB::PublicationNode,
 	public PublisherBase,
 	public ListNode<Publisher *>
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index ef03922ad3..88cc86ab80 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -57,11 +57,11 @@ namespace px4
 /**
  * Untemplated subscriber base class
  * */
-class SubscriberBase
+class __EXPORT SubscriberBase
 {
 public:
 	SubscriberBase() {};
-	~SubscriberBase() {};
+	virtual ~SubscriberBase() {};
 
 };
 
@@ -69,25 +69,25 @@ public:
  * Subscriber class which is used by nodehandle
  */
 template<typename T>
-class Subscriber :
+class __EXPORT Subscriber :
 	public SubscriberBase
 {
 public:
 	Subscriber() :
-		SubscriberBase()
+		SubscriberBase(),
+		_msg_current()
 	{};
-	~Subscriber() {};
+
+	virtual ~Subscriber() {}
 
 	/* Accessors*/
 	/**
 	 * Get the last message value
 	 */
-	virtual T get() = 0;
+	virtual T get() {return _msg_current;}
 
-	/**
-	 * Get void pointer to last message value
-	 */
-	virtual void *get_void_ptr() = 0;
+protected:
+	T _msg_current;				/**< Current Message value */
 };
 
 #if defined(__PX4_ROS)
@@ -104,11 +104,11 @@ public:
 	/**
 	 * Construct Subscriber by providing a callback function
 	 */
-	SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+	// SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+	SubscriberROS(std::function<void(const T &)> cbf) :
 		Subscriber<T>(),
 		_ros_sub(),
-		_cbf(cbf),
-		_msg_current()
+		_cbf(cbf)
 	{}
 
 	/**
@@ -117,35 +117,26 @@ public:
 	SubscriberROS() :
 		Subscriber<T>(),
 		_ros_sub(),
-		_cbf(NULL),
-		_msg_current()
+		_cbf(NULL)
 	{}
 
 
-	~SubscriberROS() {};
-
-	/* Accessors*/
-	/**
-	 * Get the last message value
-	 */
-	T get() { return _msg_current; }
-	/**
-	 * Get void pointer to last message value
-	 */
-	void *get_void_ptr() { return (void *)&_msg_current; }
+	virtual ~SubscriberROS() {};
 
 protected:
 	/**
 	 * Called on topic update, saves the current message and then calls the provided callback function
+	 * needs to use the native type as it is called by ROS
 	 */
 	void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
 	{
 		/* Store data */
-		_msg_current = (T)msg;
+		this->_msg_current = (T)msg;
 
 		/* Call callback */
 		if (_cbf != NULL) {
-			_cbf(msg);
+			// _cbf(_msg_current);
+			_cbf(this->get());
 		}
 
 	}
@@ -159,8 +150,7 @@ protected:
 	}
 
 	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
-	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf;	/**< Callback that the user provided on the subscription */
-	T _msg_current;				/**< Current Message value */
+	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
 
 };
 
@@ -170,7 +160,7 @@ protected:
 /**
  * Because we maintain a list of subscribers we need a node class
  */
-class SubscriberNode :
+class __EXPORT SubscriberNode :
 	public ListNode<SubscriberNode *>
 {
 public:
@@ -197,7 +187,7 @@ protected:
  * Subscriber class that is templated with the uorb subscription message type
  */
 template<typename T>
-class SubscriberUORB :
+class __EXPORT SubscriberUORB :
 	public Subscriber<T>,
 	public SubscriberNode
 {
@@ -220,7 +210,7 @@ public:
 		_uorb_sub(uorb_sub)
 	{}
 
-	~SubscriberUORB() {};
+	virtual ~SubscriberUORB() {};
 
 	/**
 	 * Update Subscription
@@ -229,11 +219,10 @@ public:
 	virtual void update()
 	{
 		if (!_uorb_sub->updated()) {
-			/* Topic not updated */
+			/* Topic not updated, do not call callback */
 			return;
 		}
 
-		/* get latest data */
 		_uorb_sub->update(get_void_ptr());
 	};
 
@@ -241,11 +230,16 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
+	// T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
+	// T get() {
+		// typename std::remove_reference<decltype(((T*)nullptr)->data())>::type msg = (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub;
+		// return (T)msg;
+	// }
+
 	/**
 	 * Get void pointer to last message value
 	 */
-	void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; }
+	void *get_void_ptr() { return (void *)&(this->_msg_current.data()); }
 
 	int getUORBHandle() { return _uorb_sub->getHandle(); }
 
@@ -256,7 +250,7 @@ protected:
 
 //XXX reduce to one class with overloaded constructor?
 template<typename T>
-class SubscriberUORBCallback :
+class __EXPORT SubscriberUORBCallback :
 	public SubscriberUORB<T>
 {
 public:
@@ -275,12 +269,13 @@ public:
 		// _callback(callback)
 	// {}
 	SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
-			       std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) :
-		SubscriberUORB<T>(uorb_sub),
-		_callback(callback)
+				unsigned interval,
+			       std::function<void(const T &)> cbf) :
+		SubscriberUORB<T>(uorb_sub, interval),
+		_cbf(cbf)
 	{}
 
-	~SubscriberUORBCallback() {};
+	virtual ~SubscriberUORBCallback() {};
 
 	/**
 	 * Update Subscription
@@ -289,28 +284,28 @@ public:
 	 */
 	virtual void update()
 	{
-		if (!SubscriberUORB<T>::_uorb_sub->updated()) {
+		if (!this->_uorb_sub->updated()) {
 			/* Topic not updated, do not call callback */
 			return;
-		}
 
 		/* get latest data */
-		SubscriberUORB<T>::_uorb_sub->update();
+		this->_uorb_sub->update(this->get_void_ptr());
 
 
 		/* Check if there is a callback */
-		if (_callback == nullptr) {
+		if (_cbf == nullptr) {
 			return;
 		}
 
 		/* Call callback which performs actions based on this data */
-		_callback(SubscriberUORB<T>::getUORBData());
+		_cbf(Subscriber<T>::get());
+		}
 
 	};
 
 protected:
-	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	/**< Callback handle,
-							  called when new data is available */
+	// std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	[>*< Callback handle, called when new data is available */
+	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
 };
 #endif
 

From 632a0866ef16723dd6e1f0a2f7c575706b9e10cc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 14:34:05 +0100
Subject: [PATCH 308/416] remove unneeded functionality in template

---
 msg/templates/msg.h.template | 69 +++++++-----------------------------
 1 file changed, 12 insertions(+), 57 deletions(-)

diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index 826418d118..cc128c1ea2 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -95,17 +95,17 @@ for field in spec.parsed_fields():
 @##############################
 @{
 
-type_map = {'int8': ['int8_t', '0'],
-  'int16': ['int16_t', '0'],
-  'int32': ['int32_t', '0'],
-  'int64': ['int64_t', '0'],
-  'uint8': ['uint8_t', '0'],
-  'uint16': ['uint16_t', '0'],
-  'uint32': ['uint32_t', '0'],
-  'uint64': ['uint64_t', '0'],
-  'float32': ['float', '0.0f'],
-  'bool': ['bool', 'false'],
-  'fence_vertex': ['fence_vertex', '']}
+type_map = {'int8': 'int8_t',
+  'int16': 'int16_t',
+  'int32': 'int32_t',
+  'int64': 'int64_t',
+  'uint8': 'uint8_t',
+  'uint16': 'uint16_t',
+  'uint32': 'uint32_t',
+  'uint64': 'uint64_t',
+  'float32': 'float',
+  'bool': 'bool',
+  'fence_vertex': 'fence_vertex'}
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -129,41 +129,12 @@ def print_field_def(field):
 
   if type in type_map:
     # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type][0]
+    type_px4 = type_map[type]
   else:
     raise Exception("Type {0} not supported, add to to template file!".format(type))
 
   print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
 
-# Function to init fields
-def get_field_init(field):
-  type = field.type
-  # detect embedded types
-  sl_pos = type.find('/')
-  type_appendix = ''
-  type_prefix = ''
-  if (sl_pos >= 0):
-    type = type[sl_pos + 1:]
-    type_prefix = 'struct '
-    type_appendix = '_s'
-
-  # detect arrays
-  a_pos = type.find('[')
-  array_size = ''
-  if (a_pos >= 0):
-    # field is array
-    array_size = type[a_pos:]
-    type = type[:a_pos]
-    return '\n\t%s{},'%(field.name)
-
-  if type in type_map:
-    # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type][0]
-    init_value = type_map[type][1]
-  else:
-    raise Exception("Type {0} not supported, add to to template file!".format(type))
-
-  return '\n\t%s(%s),'%(field.name, init_value)
 }
 #ifdef __cplusplus
 @#class @(spec.short_name)_s {
@@ -178,22 +149,6 @@ for field in spec.parsed_fields():
   if (not field.is_header):
     print_field_def(field)
 }@
-
-@##ifdef __cplusplus
-@#@(spec.short_name)_s() :
-@#@{
-@#field_init = ''
-@## loop over all fields and init
-@#for field in spec.parsed_fields():
-@#  if (not field.is_header):
-@#    field_init += get_field_init(field)
-@#
-@#print(field_init[:-1])
-@#}@
-@#{}
-@#virtual ~@(spec.short_name)_s() {}
-@##endif
-
 };
 
 /**

From 1f706eeb2fd210e7059e2b58f14432c987d76abf Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 14:34:34 +0100
Subject: [PATCH 309/416] small cleanup

---
 src/examples/publisher/publisher_example.h     | 1 -
 src/examples/subscriber/subscriber_example.cpp | 3 +--
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 86586d0d34..304ecef47a 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -48,6 +48,5 @@ public:
 	int main();
 protected:
 	px4::NodeHandle _n;
-	// px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
 	px4::Publisher * _rc_channels_pub;
 };
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index c662a07ad2..fd462cd2d3 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -66,8 +66,7 @@ SubscriberExample::SubscriberExample() :
 	/* Function */
 	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
 	_n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version
-	// _n.subscribe<px4_rc_channels>(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version
-
+	
 	// [> Class Method <]
 	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
 	// [> No callback <]

From 3a38b0fe359296aa19ec43ab82743aebeadb335c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 14:35:36 +0100
Subject: [PATCH 310/416] define __EXPORT for ROS

---
 CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0ac9912df3..5488dbe9db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(px4)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 add_definitions(-D__PX4_ROS)
+add_definitions(-D__EXPORT=)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

From 779400aad3f3a97057e4377d51ff362756938241 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Jan 2015 13:39:13 +0100
Subject: [PATCH 311/416] Add hackery on NuttX header, to be removed during
 rebase -i

Conflicts:
	NuttX
---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index 574bac488f..3d8171f6ea 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit 574bac488f384ddaa344378e25653c27124a2b69
+Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0

From 1628999361f5bd939ffd6312cae1d9e67698f3d4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 15:44:15 +0100
Subject: [PATCH 312/416] Revert "Fixes to make GCC 4.9 link"

This reverts commit 85b6907e1db1a6af88fe469e8e08dbd0a9d7a2a7.
---
 makefiles/toolchain_gnu-arm-eabi.mk | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index a8b4f18110..3969804535 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -110,7 +110,9 @@ ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
 			   -fno-strength-reduce \
 			   -fomit-frame-pointer \
 			   -funsafe-math-optimizations \
-			   -fno-builtin-printf
+			   -fno-builtin-printf \
+			   -ffunction-sections \
+			   -fdata-sections
 
 # enable precise stack overflow tracking
 # note - requires corresponding support in NuttX
@@ -164,8 +166,7 @@ ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
-LIBC			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libc.a)
-EXTRA_LIBS		+= $(LIBM) $(LIBC)
+EXTRA_LIBS		+= $(LIBM)
 
 # Flags we pass to the C compiler
 #

From 1511fd7b2dd19e0ae4c63553cbaf00e2a753148f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 16:33:19 +0100
Subject: [PATCH 313/416] make handle() static

---
 src/platforms/nuttx/px4_messages/px4_rc_channels.h |  2 +-
 src/platforms/px4_message.h                        |  3 +--
 src/platforms/px4_nodehandle.h                     | 10 ++++------
 src/platforms/ros/px4_messages/px4_rc_channels.h   |  2 +-
 4 files changed, 7 insertions(+), 10 deletions(-)

diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
index cb1135eb98..2ce7bfc80a 100644
--- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h
+++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
@@ -20,7 +20,7 @@ public:
 
 	~px4_rc_channels() {}
 
-	PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);}
+	static PX4TopicHandle handle() {return ORB_ID(rc_channels);}
 };
 
 }
diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h
index c908f6fc69..bff7aa3133 100644
--- a/src/platforms/px4_message.h
+++ b/src/platforms/px4_message.h
@@ -42,7 +42,7 @@
 typedef const char* PX4TopicHandle;
 #else
 #include <uORB/uORB.h>
-typedef const struct orb_metatdata* PX4TopicHandle;
+typedef orb_id_t PX4TopicHandle;
 #endif
 
 namespace px4
@@ -70,7 +70,6 @@ public:
 
 	virtual M& data() {return _data;}
 	virtual const M& data() const {return _data;}
-	virtual PX4TopicHandle handle() = 0;
 private:
 	M _data;
 };
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 2406a4a775..5ae4f9325c 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -93,7 +93,7 @@ public:
 	Subscriber<T> *subscribe(void(*fp)(const T &))
 	{
 		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
 				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
 		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
@@ -147,7 +147,7 @@ public:
 	Publisher* advertise()
 	// Publisher<T> *advertise()
 	{
-		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>((new T())->handle(), kQueueSizeDefault);
+		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault);
 		Publisher *pub =  new Publisher(ros_pub);
 		_pubs.push_back(pub);
 		return pub;
@@ -245,8 +245,7 @@ public:
 	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &),  unsigned interval=10) //XXX interval
 	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
-		const struct orb_metadata * meta = NULL;
-		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval);
+		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);
 		// SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, callback);
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
 
@@ -300,8 +299,7 @@ public:
 	{
 		//XXX
 		// uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle());
-		const struct orb_metadata * meta = NULL;
-		uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta);
+		uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle());
 		Publisher *pub = new Publisher(uorb_pub);
 
 		_pubs.add(pub);
diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h
index a5a5ee2020..5e9dc72cda 100644
--- a/src/platforms/ros/px4_messages/px4_rc_channels.h
+++ b/src/platforms/ros/px4_messages/px4_rc_channels.h
@@ -19,7 +19,7 @@ public:
 
 	~px4_rc_channels() {}
 
-	PX4TopicHandle handle() {return "rc_channels";}
+	static PX4TopicHandle handle() {return "rc_channels";}
 };
 
 }

From 2a2594a1717a5067ac1f209974851f9512e46415 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 17:40:42 +0100
Subject: [PATCH 314/416] re-enable adding of subscriber to list

---
 src/platforms/px4_nodehandle.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 5ae4f9325c..4a1e700755 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -249,11 +249,11 @@ public:
 		// SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, callback);
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
 
-		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
-		// if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
-			// _sub_min_interval = sub_px4;
-		// }
-		// _subs.add((SubscriberNode *)sub_px4);
+		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			_sub_min_interval = sub_px4;
+		}
+		_subs.add((SubscriberNode *)sub_px4);
 
 		return (Subscriber<T> *)sub_px4;
 	}

From 02fdd48a477853a2f54c7954478a3ce5b5b3f497 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 17:41:13 +0100
Subject: [PATCH 315/416] publisher: use wrapper message type

---
 src/platforms/px4_publisher.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 6d75e28fcc..0c8dd62ba1 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -126,10 +126,10 @@ public:
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic
 	 */
-	template<typename M>
-	int publish(const M &msg)
+	template<typename T>
+	int publish(const T &msg)
 	{
-		_uorb_pub->update((void *)&msg);
+		_uorb_pub->update((void *)&(msg.data()));
 		return 0;
 	}
 

From b04fcad525c8f76419d468f43a4b159bc5200fbe Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 17:41:56 +0100
Subject: [PATCH 316/416] fix bracket position

---
 src/platforms/px4_subscriber.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 88cc86ab80..8e8b786b17 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -287,6 +287,7 @@ public:
 		if (!this->_uorb_sub->updated()) {
 			/* Topic not updated, do not call callback */
 			return;
+		}
 
 		/* get latest data */
 		this->_uorb_sub->update(this->get_void_ptr());
@@ -299,7 +300,6 @@ public:
 
 		/* Call callback which performs actions based on this data */
 		_cbf(Subscriber<T>::get());
-		}
 
 	};
 

From ed526173bb4f6a809e725aeb8214cf24042edd76 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 17:57:37 +0100
Subject: [PATCH 317/416] clean up publisher example

---
 src/examples/publisher/publisher_example.cpp | 19 ++++---------------
 1 file changed, 4 insertions(+), 15 deletions(-)

diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index e85e42b386..6b30c2fe3d 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -46,9 +46,7 @@ using namespace px4;
 PublisherExample::PublisherExample() :
 	_n(),
 	_rc_channels_pub(_n.advertise<px4_rc_channels>())
-	// _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
 {
-
 }
 
 int PublisherExample::main()
@@ -56,19 +54,10 @@ int PublisherExample::main()
 	px4::Rate loop_rate(10);
 
 	while (px4::ok()) {
-		// PX4_TOPIC_T(rc_channels) msg;
-		// msg.timestamp_last_valid = px4::get_time_micros();
-		// PX4_INFO("%llu", msg.timestamp_last_valid);
-
-		// _rc_channels_pub->publish(msg);
-
-		//XXX
-		px4_rc_channels msg2;
-		msg2.data().timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg2.data().timestamp_last_valid);
-		// msg2.pub->publish2();
-		_rc_channels_pub->publish(msg2);
-
+		px4_rc_channels msg;
+		msg.data().timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("%llu", msg.data().timestamp_last_valid);
+		_rc_channels_pub->publish(msg);
 
 		_n.spinOnce();
 		loop_rate.sleep();

From f60e65b38fd051595b465f619a19effa9c13efee Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 18:05:31 +0100
Subject: [PATCH 318/416] clean up subscriber example

---
 src/examples/subscriber/subscriber_example.cpp | 5 +----
 src/examples/subscriber/subscriber_example.h   | 3 +--
 2 files changed, 2 insertions(+), 6 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index fd462cd2d3..18f1a9eb9b 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -43,8 +43,6 @@
 
 using namespace px4;
 
-// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
-void rc_channels_callback_function(const px4_rc_channels &msg);
 void rc_channels_callback_function(const px4_rc_channels &msg) {
 	PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
 }
@@ -64,9 +62,8 @@ SubscriberExample::SubscriberExample() :
 
 	/* Do some subscriptions */
 	/* Function */
-	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
 	_n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version
-	
+
 	// [> Class Method <]
 	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
 	// [> No callback <]
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 43dafe010a..883d83be7b 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -41,7 +41,7 @@
 
 using namespace px4;
 
-void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg);
+void rc_channels_callback_function(const px4_rc_channels &msg);
 
 class SubscriberExample {
 public:
@@ -56,7 +56,6 @@ protected:
 	int32_t _interval;
 	px4_param_t _p_test_float;
 	float _test_float;
-	// px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
 	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
 	// void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);

From a761df4ffa0e77e1bd34a6e02ba621ca71523389 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 18:14:20 +0100
Subject: [PATCH 319/416] clean up px4_publisher

---
 src/platforms/px4_publisher.h | 19 -------------------
 1 file changed, 19 deletions(-)

diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 0c8dd62ba1..9115545030 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -59,20 +59,11 @@ class __EXPORT PublisherBase
 public:
 	PublisherBase() {};
 	~PublisherBase() {};
-
-	/** Publishes msg
-	 * @param msg	    the message which is published to the topic
-	 */
-	// virtual int publish2(const PX4Message * const msg) = 0;
-
 };
 
 #if defined(__PX4_ROS)
-// template <typename T>
 class Publisher :
 	public PublisherBase
-	// public PublisherBase,
-	// public T
 {
 public:
 	/**
@@ -81,7 +72,6 @@ public:
 	 */
 	Publisher(ros::Publisher ros_pub) :
 		PublisherBase(),
-		// T(),
 		_ros_pub(ros_pub)
 	{}
 
@@ -90,7 +80,6 @@ public:
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic
 	 */
-	// int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
 	template <typename T>
 	int publish(T &msg) {
 		_ros_pub.publish(msg.data());
@@ -100,7 +89,6 @@ private:
 };
 #else
 class __EXPORT Publisher :
-	// public uORB::PublicationNode,
 	public PublisherBase,
 	public ListNode<Publisher *>
 
@@ -108,14 +96,7 @@ class __EXPORT Publisher :
 public:
 	/**
 	 * Construct Publisher by providing orb meta data
-	 * @param meta	    orb metadata for the topic which is used
-	 * @param list	    publisher is added to this list
 	 */
-	// Publisher(const struct orb_metadata *meta,
-		  // List<uORB::PublicationNode *> *list) :
-		// uORB::PublicationNode(meta, list),
-		// PublisherBase()
-	// {}
 	Publisher(uORB::PublicationBase * uorb_pub) :
 		PublisherBase(),
 		_uorb_pub(uorb_pub)

From 358c91932575c191c7717d9c083611d651721476 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 18:23:18 +0100
Subject: [PATCH 320/416] clean up px4_subscriber

---
 src/platforms/px4_subscriber.h | 43 ++++++++--------------------------
 1 file changed, 10 insertions(+), 33 deletions(-)

diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 8e8b786b17..2b289771bb 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -104,7 +104,6 @@ public:
 	/**
 	 * Construct Subscriber by providing a callback function
 	 */
-	// SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
 	SubscriberROS(std::function<void(const T &)> cbf) :
 		Subscriber<T>(),
 		_ros_sub(),
@@ -195,16 +194,8 @@ public:
 
 	/**
 	 * Construct SubscriberUORB by providing orb meta data without callback
-	 * @param meta	    orb metadata for the topic which is used
 	 * @param interval	Minimal interval between calls to callback
-	 * @param list	    subscriber is added to this list
 	 */
-	// SubscriberUORB(const struct orb_metadata *meta,
-		       // unsigned interval,
-		       // List<uORB::SubscriptionNode *> *list) :
-		// Subscriber<M>(),
-		// uORB::Subscription<M>(meta, interval, list)
-	// {}
 	SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) :
 		SubscriberNode(interval),
 		_uorb_sub(uorb_sub)
@@ -227,25 +218,21 @@ public:
 	};
 
 	/* Accessors*/
-	/**
-	 * Get the last message value
-	 */
-	// T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
-	// T get() {
-		// typename std::remove_reference<decltype(((T*)nullptr)->data())>::type msg = (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub;
-		// return (T)msg;
-	// }
+	int getUORBHandle() { return _uorb_sub->getHandle(); }
+
+protected:
+	uORB::SubscriptionBase * _uorb_sub;	/**< Handle to the subscription */
+
+	typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData()
+	{
+		return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub;
+	}
 
 	/**
 	 * Get void pointer to last message value
 	 */
 	void *get_void_ptr() { return (void *)&(this->_msg_current.data()); }
 
-	int getUORBHandle() { return _uorb_sub->getHandle(); }
-
-protected:
-	uORB::SubscriptionBase * _uorb_sub;	/**< Handle to the subscription */
-	typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() { return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
 };
 
 //XXX reduce to one class with overloaded constructor?
@@ -256,18 +243,9 @@ class __EXPORT SubscriberUORBCallback :
 public:
 	/**
 	 * Construct SubscriberUORBCallback by providing orb meta data
-	 * @param meta	    orb metadata for the topic which is used
-	 * @param callback	Callback, executed on receiving a new message
+	 * @param cbf		Callback, executed on receiving a new message
 	 * @param interval	Minimal interval between calls to callback
-	 * @param list	    subscriber is added to this list
 	 */
-	// SubscriberUORBCallback(const struct orb_metadata *meta,
-			       // unsigned interval,
-			       // std::function<void(const M &)> callback,
-			       // List<uORB::SubscriptionNode *> *list) :
-		// SubscriberUORB<M>(meta, interval, list),
-		// _callback(callback)
-	// {}
 	SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
 				unsigned interval,
 			       std::function<void(const T &)> cbf) :
@@ -304,7 +282,6 @@ public:
 	};
 
 protected:
-	// std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	[>*< Callback handle, called when new data is available */
 	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
 };
 #endif

From 2b103d319c8547d0d6e9ae14a6413a787051e205 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 21 Jan 2015 18:29:22 +0100
Subject: [PATCH 321/416] clean up px4_nodehandle

---
 src/platforms/px4_nodehandle.h | 61 +---------------------------------
 1 file changed, 1 insertion(+), 60 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 4a1e700755..634e5e5db6 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -78,18 +78,7 @@ public:
 	 * @param topic		Name of the topic
 	 * @param fb		Callback, executed on receiving a new message
 	 */
-	// template<typename M>
-	// Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
-	// {
-		// SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
-		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  // (SubscriberROS<M> *)sub);
-		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		// _subs.push_back(sub);
-		// return (Subscriber<M> *)sub;
-	// }
 	template<typename T>
-	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
 	Subscriber<T> *subscribe(void(*fp)(const T &))
 	{
 		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
@@ -133,19 +122,9 @@ public:
 
 	/**
 	 * Advertise topic
-	 * @param topic		Name of the topic
 	 */
-	// template<typename M>
-	// Publisher *advertise(const char *topic)
-	// {
-		// ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
-		// Publisher *pub =  new Publisher(ros_pub);
-		// _pubs.push_back(pub);
-		// return pub;
-	// }
 	template<typename T>
 	Publisher* advertise()
-	// Publisher<T> *advertise()
 	{
 		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault);
 		Publisher *pub =  new Publisher(ros_pub);
@@ -214,39 +193,14 @@ public:
 
 	/**
 	 * Subscribe with callback to function
-	 * @param meta		Describes the topic which nodehande should subscribe to
-	 * @param callback	Callback, executed on receiving a new message
-	 * @param interval	Minimal interval between calls to callback
-	 */
-
-	// template<typename M>
-	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 // std::function<void(const M &)> callback,
-				 // unsigned interval)
-	// {
-		// SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
-
-		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
-		// if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
-			// _sub_min_interval = sub_px4;
-		// }
-
-		// return (Subscriber<M> *)sub_px4;
-	// }
-	/**
-	 * Subscribe with callback to function
-	 * @param meta		Describes the topic which nodehande should subscribe to
-	 * @param callback	Callback, executed on receiving a new message
+	 * @param fp		Callback, executed on receiving a new message
 	 * @param interval	Minimal interval between calls to callback
 	 */
 
 	template<typename T>
-	// Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
-	// Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &),  unsigned interval=10) //XXX interval
 	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
 		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);
-		// SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, callback);
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
@@ -280,19 +234,6 @@ public:
 
 	/**
 	 * Advertise topic
-	 * @param meta		Describes the topic which is advertised
-	 */
-	// template<typename M>
-	// Publisher *advertise(const struct orb_metadata *meta)
-	// {
-		// //XXX
-		// Publisher *pub = new Publisher(meta, &_pubs);
-		// return pub;
-	// }
-
-	/**
-	 * Advertise topic
-	 * @param meta		Describes the topic which is advertised
 	 */
 	template<typename T>
 	Publisher *advertise()

From 2af44f5995dd121a7ce2aefd3ab1c7d8dcf3fb8d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 22 Jan 2015 08:10:08 +0100
Subject: [PATCH 322/416] multiplatform: introduce PublisherNode class for uorb
 for more consistency

---
 src/platforms/px4_nodehandle.h |  6 +++---
 src/platforms/px4_publisher.h  | 18 +++++++++++++++++-
 src/platforms/px4_subscriber.h |  3 ---
 3 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 634e5e5db6..8fafa168a2 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -176,7 +176,7 @@ public:
 		}
 
 		/* Empty publications list */
-		Publisher *pub = _pubs.getHead();
+		PublisherNode *pub = _pubs.getHead();
 		count = 0;
 
 		while (pub != nullptr) {
@@ -185,7 +185,7 @@ public:
 				break;
 			}
 
-			Publisher *sib = pub->getSibling();
+			PublisherNode *sib = pub->getSibling();
 			delete pub;
 			pub = sib;
 		}
@@ -294,7 +294,7 @@ private:
 	static const uint16_t kMaxSubscriptions = 100;
 	static const uint16_t kMaxPublications = 100;
 	List<SubscriberNode *> _subs;		/**< Subcriptions of node */
-	List<Publisher *> _pubs;		/**< Publications of node */
+	List<PublisherNode *> _pubs;		/**< Publications of node */
 	SubscriberNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 9115545030..7195777f5b 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -88,9 +88,25 @@ private:
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
+/**
+ * Because we maintain a list of publishers we need a node class
+ */
+class __EXPORT PublisherNode :
+	public ListNode<PublisherNode *>
+{
+public:
+	PublisherNode() :
+		ListNode()
+	{}
+
+	virtual ~PublisherNode() {}
+
+	virtual void update() = 0;
+};
+
 class __EXPORT Publisher :
 	public PublisherBase,
-	public ListNode<Publisher *>
+	public PublisherNode
 
 {
 public:
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 2b289771bb..d03b3edee7 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -154,8 +154,6 @@ protected:
 };
 
 #else // Building for NuttX
-
-
 /**
  * Because we maintain a list of subscribers we need a node class
  */
@@ -181,7 +179,6 @@ protected:
 
 };
 
-
 /**
  * Subscriber class that is templated with the uorb subscription message type
  */

From 8c4fce3654bbf4cb31314f1fb596f4fd17772589 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 22 Jan 2015 09:30:43 +0100
Subject: [PATCH 323/416] multiplatform: better publisher base class

---
 makefiles/toolchain_gnu-arm-eabi.mk        | 40 ++++++++++----------
 src/examples/publisher/publisher_example.h |  3 +-
 src/platforms/px4_nodehandle.h             | 14 +++----
 src/platforms/px4_publisher.h              | 44 +++++++++++++++-------
 4 files changed, 59 insertions(+), 42 deletions(-)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 3969804535..d38ef645f6 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -125,26 +125,26 @@ ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 
 # Generic warnings
 #
-ARCHWARNINGS		 = -Wall \
-			   -Wextra \
-			   -Werror \
-			   -Wdouble-promotion \
-			   -Wshadow \
-			   -Wfloat-equal \
-			   -Wframe-larger-than=1024 \
-			   -Wpointer-arith \
-			   -Wlogical-op \
-			   -Wmissing-declarations \
-			   -Wpacked \
-			   -Wno-unused-parameter \
-			   -Werror=format-security \
-			   -Werror=array-bounds \
-			   -Wfatal-errors \
-			   -Wformat=1 \
-			   -Werror=unused-but-set-variable \
-			   -Werror=unused-variable \
-			   -Werror=double-promotion \
-			   -Werror=reorder
+# ARCHWARNINGS		 = -Wall \
+			   # -Wextra \
+			   # -Werror \
+			   # -Wdouble-promotion \
+			   # -Wshadow \
+			   # -Wfloat-equal \
+			   # -Wframe-larger-than=1024 \
+			   # -Wpointer-arith \
+			   # -Wlogical-op \
+			   # -Wmissing-declarations \
+			   # -Wpacked \
+			   # -Wno-unused-parameter \
+			   # -Werror=format-security \
+			   # -Werror=array-bounds \
+			   # -Wfatal-errors \
+			   # -Wformat=1 \
+			   # -Werror=unused-but-set-variable \
+			   # -Werror=unused-variable \
+			   # -Werror=double-promotion \
+			   # -Werror=reorder
 #   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
 #   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
 #   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 304ecef47a..4ff59a226d 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -37,6 +37,7 @@
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
+#pragma once
 #include <px4.h>
 
 class PublisherExample {
@@ -48,5 +49,5 @@ public:
 	int main();
 protected:
 	px4::NodeHandle _n;
-	px4::Publisher * _rc_channels_pub;
+	px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
 };
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 8fafa168a2..a25d578455 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -124,12 +124,12 @@ public:
 	 * Advertise topic
 	 */
 	template<typename T>
-	Publisher* advertise()
+	Publisher<T>* advertise()
 	{
 		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault);
-		Publisher *pub =  new Publisher(ros_pub);
-		_pubs.push_back(pub);
-		return pub;
+		PublisherROS<T> *pub =  new PublisherROS<T>(ros_pub);
+		_pubs.push_back((PublisherBase*)pub);
+		return (Publisher<T>*)pub;
 	}
 
 	/**
@@ -236,16 +236,16 @@ public:
 	 * Advertise topic
 	 */
 	template<typename T>
-	Publisher *advertise()
+	Publisher<T> *advertise()
 	{
 		//XXX
 		// uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle());
 		uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle());
-		Publisher *pub = new Publisher(uorb_pub);
+		PublisherUORB<T> *pub = new PublisherUORB<T>(uorb_pub);
 
 		_pubs.add(pub);
 
-		return pub;
+		return (Publisher<T>*)pub;
 	}
 
 	/**
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 7195777f5b..afedbbee77 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -61,29 +61,44 @@ public:
 	~PublisherBase() {};
 };
 
+/**
+ * Publisher base class, templated with the message type
+ * */
+template <typename T>
+class __EXPORT Publisher
+{
+public:
+	Publisher() {};
+	~Publisher() {};
+
+	virtual int publish(const T &msg)  = 0;
+};
+
 #if defined(__PX4_ROS)
-class Publisher :
-	public PublisherBase
+template <typename T>
+class PublisherROS :
+	public Publisher<T>
 {
 public:
 	/**
 	 * Construct Publisher by providing a ros::Publisher
 	 * @param ros_pub the ros publisher which will be used to perform the publications
 	 */
-	Publisher(ros::Publisher ros_pub) :
-		PublisherBase(),
+	PublisherROS(ros::Publisher ros_pub) :
+		Publisher<T>(),
 		_ros_pub(ros_pub)
 	{}
 
-	~Publisher() {};
+	~PublisherROS() {};
 
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic
 	 */
-	template <typename T>
-	int publish(T &msg) {
+	int publish(const T &msg)
+	{
 		_ros_pub.publish(msg.data());
-		return 0;}
+		return 0;
+	}
 private:
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
@@ -104,8 +119,9 @@ public:
 	virtual void update() = 0;
 };
 
-class __EXPORT Publisher :
-	public PublisherBase,
+template <typename T>
+class __EXPORT PublisherUORB :
+	public Publisher<T>,
 	public PublisherNode
 
 {
@@ -113,17 +129,17 @@ public:
 	/**
 	 * Construct Publisher by providing orb meta data
 	 */
-	Publisher(uORB::PublicationBase * uorb_pub) :
-		PublisherBase(),
+	PublisherUORB(uORB::PublicationBase * uorb_pub) :
+		Publisher<T>(),
+		PublisherNode(),
 		_uorb_pub(uorb_pub)
 	{}
 
-	~Publisher() {};
+	~PublisherUORB() {};
 
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic
 	 */
-	template<typename T>
 	int publish(const T &msg)
 	{
 		_uorb_pub->update((void *)&(msg.data()));

From 67b465d800d03c6e85504b7b3f2fcc5a29497352 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 22 Jan 2015 12:31:07 +0100
Subject: [PATCH 324/416] add iris house world launch file

---
 launch/gazebo_iris_house_world.launch | 6 ++++++
 1 file changed, 6 insertions(+)
 create mode 100644 launch/gazebo_iris_house_world.launch

diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch
new file mode 100644
index 0000000000..bfdf9fe311
--- /dev/null
+++ b/launch/gazebo_iris_house_world.launch
@@ -0,0 +1,6 @@
+<launch>
+
+<include file="$(find mav_gazebo)/launch/iris_house_world_with_joy.launch" />
+<include file="$(find px4)/launch/iris.launch" />
+
+</launch>

From 8e15a5b9d0400707f539703d01f702bee03a10db Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 22 Jan 2015 15:07:06 +0100
Subject: [PATCH 325/416] install script: add missing joy dependency

---
 Tools/ros/px4_ros_installation_ubuntu.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index 72b4f94681..3b8b29381f 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -26,7 +26,7 @@ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
 sudo apt-get -y install python-rosinstall
 
 # additional dependencies
-sudo apt-get -y install ros-indigo-octomap-msgs
+sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy
 
 ## drcsim setup (for models)
 ### add osrf repository

From 49d41773fc990a6b878543fac2c5cda328bf7d78 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 22 Jan 2015 16:47:28 +0100
Subject: [PATCH 326/416] ros wrapper port callback to methods and subscriber
 without callback

---
 .../subscriber/subscriber_example.cpp         | 20 +++++----
 src/examples/subscriber/subscriber_example.h  |  2 +-
 src/platforms/px4_nodehandle.h                | 43 +++++++++----------
 src/platforms/px4_subscriber.h                |  3 +-
 4 files changed, 34 insertions(+), 34 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 18f1a9eb9b..217e4d48fe 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -64,10 +64,11 @@ SubscriberExample::SubscriberExample() :
 	/* Function */
 	_n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version
 
-	// [> Class Method <]
-	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
-	// [> No callback <]
-	// _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+	/* No callback */
+	_sub_rc_chan = _n.subscribe<px4_rc_channels>();
+
+	/* Class Method */
+	_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this);
 
 	PX4_INFO("subscribed");
 }
@@ -76,8 +77,9 @@ SubscriberExample::SubscriberExample() :
  * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
  * Also the current value of the _sub_rc_chan subscription is printed
  */
-// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			// msg.timestamp_last_valid,
-			// _sub_rc_chan->get().data().timestamp_last_valid);
-// }
+void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
+	PX4_INFO("Callback (method): [%llu]",
+			msg.data().timestamp_last_valid);
+	PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]",
+			_sub_rc_chan->get().data().timestamp_last_valid);
+}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 883d83be7b..8da3df4387 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -58,7 +58,7 @@ protected:
 	float _test_float;
 	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
-	// void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
+	void rc_channels_callback(const px4_rc_channels &msg);
 
 
 };
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index a25d578455..e619623b9d 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -91,34 +91,33 @@ public:
 
 	/**
 	 * Subscribe with callback to class method
-	 * @param topic		Name of the topic
 	 * @param fb		Callback, executed on receiving a new message
+	 * @param obj	        pointer class instance
 	 */
-	// template<typename M, typename T>
-	// Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
-	// {
-		// SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
-		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  // (SubscriberROS<M> *)sub);
-		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		// _subs.push_back(sub);
-		// return (Subscriber<M> *)sub;
-	// }
+	template<typename T, typename C>
+	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj)
+	{
+		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, obj, std::placeholders::_1));
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
+				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
+		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
+		_subs.push_back(sub);
+		return (Subscriber<T> *)sub;
+	}
 
 	/**
 	 * Subscribe with no callback, just the latest value is stored on updates
-	 * @param topic		Name of the topic
 	 */
-	// template<typename M>
-	// Subscriber<M> *subscribe(const char *topic)
-	// {
-		// SubscriberBase *sub = new SubscriberROS<M>();
-		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  // (SubscriberROS<M> *)sub);
-		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		// _subs.push_back(sub);
-		// return (Subscriber<M> *)sub;
-	// }
+	template<typename T>
+	Subscriber<T> *subscribe()
+	{
+		SubscriberBase *sub = new SubscriberROS<T>();
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
+				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
+		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
+		_subs.push_back(sub);
+		return (Subscriber<T> *)sub;
+	}
 
 	/**
 	 * Advertise topic
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index d03b3edee7..c499712a96 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -130,11 +130,10 @@ protected:
 	void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
 	{
 		/* Store data */
-		this->_msg_current = (T)msg;
+		this->_msg_current.data() = msg;
 
 		/* Call callback */
 		if (_cbf != NULL) {
-			// _cbf(_msg_current);
 			_cbf(this->get());
 		}
 

From b2a911b88d6a541218ef6e633be0d6693de31c8e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 22 Jan 2015 17:10:20 +0100
Subject: [PATCH 327/416] uorb wrapper port callback to methods and subscriber
 without callback

---
 src/platforms/px4_nodehandle.h | 53 ++++++++++++++++++++++++----------
 1 file changed, 37 insertions(+), 16 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index e619623b9d..d78c865de6 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -81,8 +81,7 @@ public:
 	template<typename T>
 	Subscriber<T> *subscribe(void(*fp)(const T &))
 	{
-		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
+		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1)); 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
 				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
 		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
@@ -99,7 +98,8 @@ public:
 	{
 		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, obj, std::placeholders::_1));
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
-				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
+				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);//XXX needs cleanup in destructor ore move into class
+
 		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
 		return (Subscriber<T> *)sub;
@@ -199,7 +199,7 @@ public:
 	template<typename T>
 	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
-		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);
+		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
@@ -211,25 +211,46 @@ public:
 		return (Subscriber<T> *)sub_px4;
 	}
 
+	/**
+	 * Subscribe with callback to class method
+	 * @param fb		Callback, executed on receiving a new message
+	 * @param obj	        pointer class instance
+	 */
+	template<typename T, typename C>
+	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10)
+	{
+		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1));
+
+		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			_sub_min_interval = sub_px4;
+		}
+		_subs.add((SubscriberNode *)sub_px4);
+
+		return (Subscriber<T> *)sub_px4;
+	}
+
 	/**
 	 * Subscribe without callback to function
-	 * @param meta		Describes the topic which nodehande should subscribe to
 	 * @param interval	Minimal interval between data fetches from orb
 	 */
 
-	// template<typename M>
-	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 // unsigned interval)
-	// {
-		// SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
+	template<typename T>
+	Subscriber<T> *subscribe(unsigned interval=10) //XXX interval
+	{
+		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
 
-		// [> Check if this is the smallest interval so far and update _sub_min_interval <]
-		// if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
-			// _sub_min_interval = sub_px4;
-		// }
+		SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(uorb_sub, interval);
 
-		// return (Subscriber<M> *)sub_px4;
-	// }
+		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			_sub_min_interval = sub_px4;
+		}
+		_subs.add((SubscriberNode *)sub_px4);
+
+		return (Subscriber<T> *)sub_px4;
+	}
 
 	/**
 	 * Advertise topic

From 1e504478a00f08c4d7ab381aa9bec5cdbee5513f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 07:06:02 +0100
Subject: [PATCH 328/416] move ros::subscriber into constructor of subscriber

---
 src/platforms/px4_nodehandle.h | 15 +++------------
 src/platforms/px4_subscriber.h | 29 ++++++++++-------------------
 2 files changed, 13 insertions(+), 31 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index d78c865de6..40604aa86e 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -81,9 +81,7 @@ public:
 	template<typename T>
 	Subscriber<T> *subscribe(void(*fp)(const T &))
 	{
-		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1)); 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
-				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
-		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
+		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1));
 		_subs.push_back(sub);
 		return (Subscriber<T> *)sub;
 	}
@@ -96,11 +94,7 @@ public:
 	template<typename T, typename C>
 	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj)
 	{
-		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, obj, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
-				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);//XXX needs cleanup in destructor ore move into class
-
-		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
+		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1));
 		_subs.push_back(sub);
 		return (Subscriber<T> *)sub;
 	}
@@ -111,10 +105,7 @@ public:
 	template<typename T>
 	Subscriber<T> *subscribe()
 	{
-		SubscriberBase *sub = new SubscriberROS<T>();
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault,
-				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
-		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
+		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this);
 		_subs.push_back(sub);
 		return (Subscriber<T> *)sub;
 	}
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index c499712a96..a54b8eb087 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -102,24 +102,23 @@ class SubscriberROS :
 
 public:
 	/**
-	 * Construct Subscriber by providing a callback function
+	 * Construct Subscriber without a callback function
 	 */
-	SubscriberROS(std::function<void(const T &)> cbf) :
-		Subscriber<T>(),
-		_ros_sub(),
-		_cbf(cbf)
+	SubscriberROS(ros::NodeHandle *rnh) :
+		px4::Subscriber<T>(),
+		_cbf(NULL),
+		_ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS<T>::callback, this))
 	{}
 
 	/**
-	 * Construct Subscriber without a callback function
+	 * Construct Subscriber by providing a callback function
 	 */
-	SubscriberROS() :
-		Subscriber<T>(),
-		_ros_sub(),
-		_cbf(NULL)
+	//XXX queue default
+	SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) :
+		_cbf(cbf),
+		_ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS<T>::callback, this))
 	{}
 
-
 	virtual ~SubscriberROS() {};
 
 protected:
@@ -139,14 +138,6 @@ protected:
 
 	}
 
-	/**
-	 * Saves the ros subscriber to keep ros subscription alive
-	 */
-	void set_ros_sub(ros::Subscriber ros_sub)
-	{
-		_ros_sub = ros_sub;
-	}
-
 	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
 	std::function<void(const T &)> _cbf;	/**< Callback that the user provided on the subscription */
 

From 1ad6e00234e990d44f2a9ca93bcc50696c4c530c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 07:16:07 +0100
Subject: [PATCH 329/416] re-enable warnings/errors

---
 makefiles/toolchain_gnu-arm-eabi.mk | 40 ++++++++++++++---------------
 1 file changed, 20 insertions(+), 20 deletions(-)

diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index d38ef645f6..3969804535 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -125,26 +125,26 @@ ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 
 # Generic warnings
 #
-# ARCHWARNINGS		 = -Wall \
-			   # -Wextra \
-			   # -Werror \
-			   # -Wdouble-promotion \
-			   # -Wshadow \
-			   # -Wfloat-equal \
-			   # -Wframe-larger-than=1024 \
-			   # -Wpointer-arith \
-			   # -Wlogical-op \
-			   # -Wmissing-declarations \
-			   # -Wpacked \
-			   # -Wno-unused-parameter \
-			   # -Werror=format-security \
-			   # -Werror=array-bounds \
-			   # -Wfatal-errors \
-			   # -Wformat=1 \
-			   # -Werror=unused-but-set-variable \
-			   # -Werror=unused-variable \
-			   # -Werror=double-promotion \
-			   # -Werror=reorder
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Werror \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter \
+			   -Werror=format-security \
+			   -Werror=array-bounds \
+			   -Wfatal-errors \
+			   -Wformat=1 \
+			   -Werror=unused-but-set-variable \
+			   -Werror=unused-variable \
+			   -Werror=double-promotion \
+			   -Werror=reorder
 #   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
 #   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
 #   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives

From 2dfd30c25e2839b762ca127fd4af696284df34b9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 07:17:06 +0100
Subject: [PATCH 330/416] move uorb::subscriptionbase into constructor of
 subscriber

---
 src/platforms/px4_nodehandle.h | 10 +++-------
 src/platforms/px4_subscriber.h | 13 +++++++------
 2 files changed, 10 insertions(+), 13 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 40604aa86e..f2d09c15cb 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -190,8 +190,7 @@ public:
 	template<typename T>
 	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
-		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
-		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1));
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
 		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
@@ -210,8 +209,7 @@ public:
 	template<typename T, typename C>
 	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10)
 	{
-		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
-		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1));
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
 		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
@@ -230,9 +228,7 @@ public:
 	template<typename T>
 	Subscriber<T> *subscribe(unsigned interval=10) //XXX interval
 	{
-		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class
-
-		SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(uorb_sub, interval);
+		SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
 
 		/* Check if this is the smallest interval so far and update _sub_min_interval */
 		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index a54b8eb087..e086cd4c26 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -183,12 +183,14 @@ public:
 	 * Construct SubscriberUORB by providing orb meta data without callback
 	 * @param interval	Minimal interval between calls to callback
 	 */
-	SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) :
+	SubscriberUORB(unsigned interval) :
 		SubscriberNode(interval),
-		_uorb_sub(uorb_sub)
+		_uorb_sub(new uORB::SubscriptionBase(T::handle(), interval))
 	{}
 
-	virtual ~SubscriberUORB() {};
+	virtual ~SubscriberUORB() {
+		delete _uorb_sub;
+	};
 
 	/**
 	 * Update Subscription
@@ -233,10 +235,9 @@ public:
 	 * @param cbf		Callback, executed on receiving a new message
 	 * @param interval	Minimal interval between calls to callback
 	 */
-	SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
-				unsigned interval,
+	SubscriberUORBCallback(unsigned interval,
 			       std::function<void(const T &)> cbf) :
-		SubscriberUORB<T>(uorb_sub, interval),
+		SubscriberUORB<T>(interval),
 		_cbf(cbf)
 	{}
 

From 59f05a7195ae4a2d57e276c31e12bcf6af477408 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 08:03:07 +0100
Subject: [PATCH 331/416] move ros::publisher into constructor of publisher

---
 src/platforms/px4_nodehandle.h | 4 +---
 src/platforms/px4_publisher.h  | 7 ++++---
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index f2d09c15cb..c85b8118b3 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -116,8 +116,7 @@ public:
 	template<typename T>
 	Publisher<T>* advertise()
 	{
-		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault);
-		PublisherROS<T> *pub =  new PublisherROS<T>(ros_pub);
+		PublisherROS<T> *pub =  new PublisherROS<T>((ros::NodeHandle*)this);
 		_pubs.push_back((PublisherBase*)pub);
 		return (Publisher<T>*)pub;
 	}
@@ -134,7 +133,6 @@ public:
 
 
 private:
-	static const uint32_t kQueueSizeDefault = 1;		/**< Size of queue for ROS */
 	std::list<SubscriberBase *> _subs;				/**< Subcriptions of node */
 	std::list<PublisherBase *> _pubs;				/**< Publications of node */
 };
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index afedbbee77..ea675e67be 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -84,9 +84,9 @@ public:
 	 * Construct Publisher by providing a ros::Publisher
 	 * @param ros_pub the ros publisher which will be used to perform the publications
 	 */
-	PublisherROS(ros::Publisher ros_pub) :
+	PublisherROS(ros::NodeHandle *rnh) :
 		Publisher<T>(),
-		_ros_pub(ros_pub)
+		_ros_pub(rnh->advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault))
 	{}
 
 	~PublisherROS() {};
@@ -99,7 +99,8 @@ public:
 		_ros_pub.publish(msg.data());
 		return 0;
 	}
-private:
+protected:
+	static const uint32_t kQueueSizeDefault = 1;		/**< Size of queue for ROS */
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else

From 16c85c6d1870f6f410cf75fc0fe3cb386eb9f6ee Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 08:03:26 +0100
Subject: [PATCH 332/416] move uorb::publisherbase into constructor of
 publisher

---
 src/platforms/px4_nodehandle.h | 7 +------
 src/platforms/px4_publisher.h  | 4 ++--
 src/platforms/px4_subscriber.h | 7 ++++---
 3 files changed, 7 insertions(+), 11 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index c85b8118b3..fd1647051a 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -243,13 +243,8 @@ public:
 	template<typename T>
 	Publisher<T> *advertise()
 	{
-		//XXX
-		// uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle());
-		uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle());
-		PublisherUORB<T> *pub = new PublisherUORB<T>(uorb_pub);
-
+		PublisherUORB<T> *pub = new PublisherUORB<T>();
 		_pubs.add(pub);
-
 		return (Publisher<T>*)pub;
 	}
 
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index ea675e67be..a9e40e8d67 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -130,10 +130,10 @@ public:
 	/**
 	 * Construct Publisher by providing orb meta data
 	 */
-	PublisherUORB(uORB::PublicationBase * uorb_pub) :
+	PublisherUORB() :
 		Publisher<T>(),
 		PublisherNode(),
-		_uorb_pub(uorb_pub)
+		_uorb_pub(new uORB::PublicationBase(T::handle()))
 	{}
 
 	~PublisherUORB() {};
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index e086cd4c26..17a95f12d8 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -107,21 +107,22 @@ public:
 	SubscriberROS(ros::NodeHandle *rnh) :
 		px4::Subscriber<T>(),
 		_cbf(NULL),
-		_ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS<T>::callback, this))
+		_ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
 	{}
 
 	/**
 	 * Construct Subscriber by providing a callback function
 	 */
-	//XXX queue default
 	SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) :
 		_cbf(cbf),
-		_ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS<T>::callback, this))
+		_ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
 	{}
 
 	virtual ~SubscriberROS() {};
 
 protected:
+	static const uint32_t kQueueSizeDefault = 1;		/**< Size of queue for ROS */
+
 	/**
 	 * Called on topic update, saves the current message and then calls the provided callback function
 	 * needs to use the native type as it is called by ROS

From 57569482ad5739bb4471826014818c92bdb33c6d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 08:16:07 +0100
Subject: [PATCH 333/416] remove unneeded friend

---
 src/platforms/px4_subscriber.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 17a95f12d8..6a81ef8d22 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -98,8 +98,6 @@ template<typename T>
 class SubscriberROS :
 	public Subscriber<T>
 {
-	friend class NodeHandle;
-
 public:
 	/**
 	 * Construct Subscriber without a callback function

From af943cf16adb79ab33097bd560a85fee203a5215 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 08:27:06 +0100
Subject: [PATCH 334/416] add update_sub_min_interval function

---
 src/platforms/px4_nodehandle.h | 36 +++++++++++++++-------------------
 1 file changed, 16 insertions(+), 20 deletions(-)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index fd1647051a..2f92ead5a1 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -132,7 +132,7 @@ public:
 	void spin() { ros::spin(); }
 
 
-private:
+protected:
 	std::list<SubscriberBase *> _subs;				/**< Subcriptions of node */
 	std::list<PublisherBase *> _pubs;				/**< Publications of node */
 };
@@ -189,13 +189,8 @@ public:
 	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
 	{
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
-
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
-			_sub_min_interval = sub_px4;
-		}
+		update_sub_min_interval(interval, sub_px4);
 		_subs.add((SubscriberNode *)sub_px4);
-
 		return (Subscriber<T> *)sub_px4;
 	}
 
@@ -208,13 +203,8 @@ public:
 	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10)
 	{
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
-
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
-			_sub_min_interval = sub_px4;
-		}
+		update_sub_min_interval(interval, sub_px4);
 		_subs.add((SubscriberNode *)sub_px4);
-
 		return (Subscriber<T> *)sub_px4;
 	}
 
@@ -227,13 +217,8 @@ public:
 	Subscriber<T> *subscribe(unsigned interval=10) //XXX interval
 	{
 		SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
-
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
-		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
-			_sub_min_interval = sub_px4;
-		}
+		update_sub_min_interval(interval, sub_px4);
 		_subs.add((SubscriberNode *)sub_px4);
-
 		return (Subscriber<T> *)sub_px4;
 	}
 
@@ -290,13 +275,24 @@ public:
 			spinOnce();
 		}
 	}
-private:
+protected:
 	static const uint16_t kMaxSubscriptions = 100;
 	static const uint16_t kMaxPublications = 100;
 	List<SubscriberNode *> _subs;		/**< Subcriptions of node */
 	List<PublisherNode *> _pubs;		/**< Publications of node */
 	SubscriberNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
+
+	/**
+	 * Check if this is the smallest interval so far and update _sub_min_interval
+	 */
+	template<typename T>
+	void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub)
+	{
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+			_sub_min_interval = sub;
+		}
+	}
 };
 #endif
 }

From da7ad9a3329db43144b5ca3e60c6c56d22fdc3d4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 09:02:04 +0100
Subject: [PATCH 335/416] multiplatform: re-enable interval functionality (for
 uorb)

---
 src/examples/subscriber/subscriber_example.cpp |  6 +++---
 src/platforms/px4_nodehandle.h                 | 15 +++++++++------
 2 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 217e4d48fe..215336c174 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -62,13 +62,13 @@ SubscriberExample::SubscriberExample() :
 
 	/* Do some subscriptions */
 	/* Function */
-	_n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval);
 
 	/* No callback */
-	_sub_rc_chan = _n.subscribe<px4_rc_channels>();
+	_sub_rc_chan = _n.subscribe<px4_rc_channels>(500);
 
 	/* Class Method */
-	_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this);
+	_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000);
 
 	PX4_INFO("subscribed");
 }
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 2f92ead5a1..80be9ec527 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -79,7 +79,7 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename T>
-	Subscriber<T> *subscribe(void(*fp)(const T &))
+	Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval)
 	{
 		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1));
 		_subs.push_back(sub);
@@ -92,7 +92,7 @@ public:
 	 * @param obj	        pointer class instance
 	 */
 	template<typename T, typename C>
-	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj)
+	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
 	{
 		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1));
 		_subs.push_back(sub);
@@ -103,7 +103,7 @@ public:
 	 * Subscribe with no callback, just the latest value is stored on updates
 	 */
 	template<typename T>
-	Subscriber<T> *subscribe()
+	Subscriber<T> *subscribe(unsigned interval)
 	{
 		SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this);
 		_subs.push_back(sub);
@@ -186,8 +186,9 @@ public:
 	 */
 
 	template<typename T>
-	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval=10) //XXX interval
+	Subscriber<T> *subscribe(void(*fp)(const T &),  unsigned interval)
 	{
+		(void)interval;
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
 		update_sub_min_interval(interval, sub_px4);
 		_subs.add((SubscriberNode *)sub_px4);
@@ -200,8 +201,9 @@ public:
 	 * @param obj	        pointer class instance
 	 */
 	template<typename T, typename C>
-	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10)
+	Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
 	{
+		(void)interval;
 		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
 		update_sub_min_interval(interval, sub_px4);
 		_subs.add((SubscriberNode *)sub_px4);
@@ -214,8 +216,9 @@ public:
 	 */
 
 	template<typename T>
-	Subscriber<T> *subscribe(unsigned interval=10) //XXX interval
+	Subscriber<T> *subscribe(unsigned interval)
 	{
+		(void)interval;
 		SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
 		update_sub_min_interval(interval, sub_px4);
 		_subs.add((SubscriberNode *)sub_px4);

From c33173cd5de81a9fb32757c3a3c662716f76b300 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 09:23:29 +0100
Subject: [PATCH 336/416] uorb pub: cleanup objects

---
 src/platforms/px4_publisher.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index a9e40e8d67..d9cd7a3c19 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -136,7 +136,9 @@ public:
 		_uorb_pub(new uORB::PublicationBase(T::handle()))
 	{}
 
-	~PublisherUORB() {};
+	~PublisherUORB() {
+		delete _uorb_pub;
+	};
 
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic

From 6f7fa3b4e7e6fd387530a60c800992c5d7bab87b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 10:33:18 +0100
Subject: [PATCH 337/416] header generation script: add option to set output
 filename prefix

---
 Tools/px_generate_uorb_topic_headers.py | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
index 2ddbd69847..54430cc38d 100755
--- a/Tools/px_generate_uorb_topic_headers.py
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -60,7 +60,7 @@ def convert_file(filename, outputdir, templatedir, includepath):
         """
         Converts a single .msg file to a uorb header
         """
-        print("Generating uORB headers from {0}".format(filename))
+        print("Generating headers from {0}".format(filename))
         genmsg.template_tools.generate_from_file(filename,
                                                  package,
                                                  outputdir,
@@ -85,7 +85,7 @@ def convert_dir(inputdir, outputdir, templatedir):
                             includepath)
 
 
-def copy_changed(inputdir, outputdir):
+def copy_changed(inputdir, outputdir, prefix=''):
         """
         Copies files from inputdir to outputdir if they don't exist in
         ouputdir or if their content changed
@@ -94,7 +94,7 @@ def copy_changed(inputdir, outputdir):
                 fni = os.path.join(inputdir, f)
                 if os.path.isfile(fni):
                         # Check if f exists in outpoutdir, copy the file if not
-                        fno = os.path.join(outputdir, f)
+                        fno = os.path.join(outputdir, prefix + f)
                         if not os.path.isfile(fno):
                                 shutil.copy(fni, fno)
                                 print("{0}: new header file".format(f))
@@ -108,7 +108,8 @@ def copy_changed(inputdir, outputdir):
 
                         print("{0}: unchanged".format(f))
 
-def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
+
+def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix):
         """
         Converts all .msg files in inputdir to uORB header files
         Unchanged existing files are not overwritten.
@@ -117,7 +118,7 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
         convert_dir(inputdir, temporarydir, templatedir)
 
         # Copy changed headers from temporary dir to output dir
-        copy_changed(temporarydir, outputdir)
+        copy_changed(temporarydir, outputdir, prefix)
 
 if __name__ == "__main__":
         parser = argparse.ArgumentParser(
@@ -132,6 +133,9 @@ if __name__ == "__main__":
                             help='output directory for header files')
         parser.add_argument('-t', dest='temporarydir',
                             help='temporary directory')
+        parser.add_argument('-p', dest='prefix', default='',
+                            help='string added as prefix to the output file '
+                            ' name when converting directories')
         args = parser.parse_args()
 
         if args.file is not None:
@@ -146,4 +150,5 @@ if __name__ == "__main__":
                     args.dir,
                     args.outputdir,
                     args.templatedir,
-                    args.temporarydir)
+                    args.temporarydir,
+                    args.prefix)

From 738f65a705c5dd2e664fedf54fd1064f685ff5c7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 10:34:04 +0100
Subject: [PATCH 338/416] generate wrapper headers for uorb

---
 .gitignore                                    |   1 +
 Makefile                                      |  15 +-
 msg/templates/px4/msg.h.template              |  95 +++++++++++
 msg/templates/uorb/msg.h.template             | 159 ++++++++++++++++++
 .../nuttx/px4_messages/px4_rc_channels.h      |  26 ---
 5 files changed, 266 insertions(+), 30 deletions(-)
 create mode 100644 msg/templates/px4/msg.h.template
 create mode 100644 msg/templates/uorb/msg.h.template
 delete mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h

diff --git a/.gitignore b/.gitignore
index 1c4f49fdea..764be0029f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -40,6 +40,7 @@ tags
 .ropeproject
 *.orig
 src/modules/uORB/topics/*
+src/platforms/nuttx/px4_messages/*
 Firmware.zip
 unittests/build
 *.generated.h
diff --git a/Makefile b/Makefile
index 6480286d2d..bffba69aed 100644
--- a/Makefile
+++ b/Makefile
@@ -225,9 +225,12 @@ updatesubmodules:
 	$(Q) (git submodule update)
 
 MSG_DIR = $(PX4_BASE)msg
-MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
+UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb
+MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
-TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
+MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages
+MULTIPLATFORM_PREFIX = px4_
+TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary
 GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src
 GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src
 
@@ -236,9 +239,13 @@ generateuorbtopicheaders:
 	@$(ECHO) "Generating uORB topic headers"
 	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
 		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
-		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
+		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR))
+	@$(ECHO) "Generating multiplatform uORB topic wrapper headers"
+	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
+		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
+		-d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX))
 # clean up temporary files
-	$(Q) (rm -r $(TOPICS_TEMPORARY_DIR))
+	$(Q) (rm -r $(TOPICHEADER_TEMP_DIR))
 
 #
 # Testing targets
diff --git a/msg/templates/px4/msg.h.template b/msg/templates/px4/msg.h.template
new file mode 100644
index 0000000000..ba0fbd4bfe
--- /dev/null
+++ b/msg/templates/px4/msg.h.template
@@ -0,0 +1,95 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# This file generates the multiplatform wrapper
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_class = 'px4_%s'%spec.short_name
+uorb_struct = '%s_s'%spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <uORB/topics/@(topic_name).h>
+#include "platforms/px4_message.h"
+
+@##############################
+@# Class
+@##############################
+
+namespace px4
+{
+
+class __EXPORT @(cpp_class) :
+	public PX4Message<@(uorb_struct)>
+{
+public:
+	@(cpp_class)() :
+		PX4Message<@(uorb_struct)>()
+	{}
+
+	@(cpp_class)(@(uorb_struct) msg) :
+		PX4Message<@(uorb_struct)>(msg)
+	{}
+
+	~@(cpp_class)() {}
+
+	static PX4TopicHandle handle() {return ORB_ID(@(topic_name));}
+};
+
+};
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
new file mode 100644
index 0000000000..11cb1ea2ae
--- /dev/null
+++ b/msg/templates/uorb/msg.h.template
@@ -0,0 +1,159 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace
+cpp_class = '%s_'%spec.short_name
+cpp_full_name = '%s%s'%(cpp_namespace,cpp_class)
+cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name)
+cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <stdint.h>
+#include <uORB/uORB.h>
+
+@##############################
+@# Includes for dependencies
+@##############################
+@{
+for field in spec.parsed_fields():
+  if (not field.is_builtin):
+    if (not field.is_header):
+      (package, name) = genmsg.names.package_resource_name(field.base_type)
+      package = package or spec.package # convert '' to package
+      print('#include <uORB/topics/%s.h>'%(name))
+
+}@
+@# Constants
+@[for constant in spec.constants]@
+#define @(constant.name) @(int(constant.val))
+@[end for]
+
+/**
+ * @@addtogroup topics
+ * @@{
+ */
+
+@##############################
+@# Main struct of message
+@##############################
+@{
+
+type_map = {'int8': 'int8_t',
+  'int16': 'int16_t',
+  'int32': 'int32_t',
+  'int64': 'int64_t',
+  'uint8': 'uint8_t',
+  'uint16': 'uint16_t',
+  'uint32': 'uint32_t',
+  'uint64': 'uint64_t',
+  'float32': 'float',
+  'bool': 'bool',
+  'fence_vertex': 'fence_vertex'}
+
+# Function to print a standard ros type
+def print_field_def(field):
+  type = field.type
+  # detect embedded types
+  sl_pos = type.find('/')
+  type_appendix = ''
+  type_prefix = ''
+  if (sl_pos >= 0):
+    type = type[sl_pos + 1:]
+    type_prefix = 'struct '
+    type_appendix = '_s'
+
+  # detect arrays
+  a_pos = type.find('[')
+  array_size = ''
+  if (a_pos >= 0):
+    # field is array
+    array_size = type[a_pos:]
+    type = type[:a_pos]
+
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
+
+}
+#ifdef __cplusplus
+@#class @(spec.short_name)_s {
+struct __EXPORT @(spec.short_name)_s {
+@#public:
+#else
+struct @(spec.short_name)_s {
+#endif
+@{
+# loop over all fields and print the type and name
+for field in spec.parsed_fields():
+  if (not field.is_header):
+    print_field_def(field)
+}@
+};
+
+/**
+ * @@}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(@(spec.short_name));
diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
deleted file mode 100644
index 2ce7bfc80a..0000000000
--- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#include <uORB/uORB.h>
-#include <uORB/topics/rc_channels.h>
-#include "platforms/px4_message.h"
-
-#pragma once
-namespace px4
-{
-
-class __EXPORT px4_rc_channels :
-	public PX4Message<rc_channels_s>
-{
-public:
-	px4_rc_channels() :
-		PX4Message<rc_channels_s>()
-	{}
-
-	px4_rc_channels(rc_channels_s msg) :
-		PX4Message<rc_channels_s>(msg)
-	{}
-
-	~px4_rc_channels() {}
-
-	static PX4TopicHandle handle() {return ORB_ID(rc_channels);}
-};
-
-}

From 5e9bef6f4b710aae319bd60068107bff41003fd8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 10:37:17 +0100
Subject: [PATCH 339/416] cleanup template for normal uorb headers

---
 msg/templates/uorb/msg.h.template | 17 +++++++----------
 1 file changed, 7 insertions(+), 10 deletions(-)

diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 11cb1ea2ae..622641617f 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -47,17 +47,14 @@
  *
  ****************************************************************************/
 
- /* Auto-generated by genmsg_cpp from file @file_name_in */
+/* Auto-generated by genmsg_cpp from file @file_name_in */
 
 @{
 import genmsg.msgs
 import gencpp
 
-cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace
-cpp_class = '%s_'%spec.short_name
-cpp_full_name = '%s%s'%(cpp_namespace,cpp_class)
-cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name)
-cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
+uorb_struct = '%s_s'%spec.short_name
+topic_name = spec.short_name
 }@
 
 #pragma once
@@ -137,11 +134,11 @@ def print_field_def(field):
 
 }
 #ifdef __cplusplus
-@#class @(spec.short_name)_s {
-struct __EXPORT @(spec.short_name)_s {
+@#class @(uorb_struct) {
+struct __EXPORT @(uorb_struct) {
 @#public:
 #else
-struct @(spec.short_name)_s {
+struct @(uorb_struct) {
 #endif
 @{
 # loop over all fields and print the type and name
@@ -156,4 +153,4 @@ for field in spec.parsed_fields():
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(@(spec.short_name));
+ORB_DECLARE(@(topic_name));

From 4ba57ad285884a2b01ebf8aac2c710ed63f7ffd3 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 11:39:10 +0100
Subject: [PATCH 340/416] get rid of unnecessary defines

---
 src/platforms/px4_defines.h    | 1 -
 src/platforms/px4_middleware.h | 4 ----
 2 files changed, 5 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 6b6a03893b..325832f0f1 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -50,7 +50,6 @@
 /*
  * Building for running within the ROS environment
  */
-#define __EXPORT
 #define noreturn_function
 #ifdef __cplusplus
 #include "ros/ros.h"
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index cd52fd650b..735d34234a 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -42,10 +42,6 @@
 #include <stdint.h>
 #include <unistd.h>
 
-#if defined(__PX4_ROS)
-#define __EXPORT
-#endif
-
 namespace px4
 {
 

From d7e57061c976ba18c69d8be9949660e85f645126 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 11:40:23 +0100
Subject: [PATCH 341/416] generate message wrapper headers on ros

---
 .gitignore                                    |  1 +
 CMakeLists.txt                                | 16 +++-
 Makefile                                      |  6 +-
 msg/templates/px4/ros/msg.h.template          | 95 +++++++++++++++++++
 msg/templates/px4/{ => uorb}/msg.h.template   | 10 +-
 .../ros/px4_messages/px4_rc_channels.h        | 25 -----
 6 files changed, 117 insertions(+), 36 deletions(-)
 create mode 100644 msg/templates/px4/ros/msg.h.template
 rename msg/templates/px4/{ => uorb}/msg.h.template (94%)
 delete mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h

diff --git a/.gitignore b/.gitignore
index 764be0029f..1c3f8fe047 100644
--- a/.gitignore
+++ b/.gitignore
@@ -41,6 +41,7 @@ tags
 *.orig
 src/modules/uORB/topics/*
 src/platforms/nuttx/px4_messages/*
+src/platforms/ros/px4_messages/*
 Firmware.zip
 unittests/build
 *.generated.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5488dbe9db..25822d7190 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -125,6 +125,16 @@ include_directories(
   ${EIGEN_INCLUDE_DIRS}
 )
 
+## generate multiplatform wrapper headers
+## note that the message header files are generated as in any ROS project with generate_messages()
+set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages)
+set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
+set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
+set(MULTIPLATFORM_PREFIX px4_)
+add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py
+	-d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
+	-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
+
 ## Declare a cpp library
 add_library(px4
   src/platforms/ros/px4_ros_impl.cpp
@@ -133,7 +143,7 @@ add_library(px4
   src/lib/mathlib/math/Limits.cpp
   src/modules/systemlib/circuit_breaker.cpp
 )
-add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
 
 target_link_libraries(px4
   ${catkin_LIBRARIES}
@@ -143,7 +153,7 @@ target_link_libraries(px4
 add_executable(publisher
 	src/examples/publisher/publisher_main.cpp
 	src/examples/publisher/publisher_example.cpp)
-add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
 target_link_libraries(publisher
   ${catkin_LIBRARIES}
   px4
@@ -153,7 +163,7 @@ target_link_libraries(publisher
 add_executable(subscriber
 	src/examples/subscriber/subscriber_main.cpp
 	src/examples/subscriber/subscriber_example.cpp)
-add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
 target_link_libraries(subscriber
   ${catkin_LIBRARIES}
   px4
diff --git a/Makefile b/Makefile
index bffba69aed..c727c9a697 100644
--- a/Makefile
+++ b/Makefile
@@ -226,13 +226,13 @@ updatesubmodules:
 
 MSG_DIR = $(PX4_BASE)msg
 UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb
-MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4
+MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
 MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages
 MULTIPLATFORM_PREFIX = px4_
 TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary
-GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src
-GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src
+GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src
+GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src
 
 .PHONY: generateuorbtopicheaders
 generateuorbtopicheaders:
diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template
new file mode 100644
index 0000000000..176be0d091
--- /dev/null
+++ b/msg/templates/px4/ros/msg.h.template
@@ -0,0 +1,95 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# This file generates the multiplatform wrapper
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+cpp_class = 'px4_%s'%spec.short_name
+native_type = spec.short_name
+topic_name = spec.short_name
+}@
+
+#pragma once
+
+@##############################
+@# Generic Includes
+@##############################
+#include <uORB/topics/@(topic_name).h>
+#include "platforms/px4_message.h"
+
+@##############################
+@# Class
+@##############################
+
+namespace px4
+{
+
+class @(cpp_class) :
+	public PX4Message<@(native_type)>
+{
+public:
+	@(cpp_class)() :
+		PX4Message<@(native_type)>()
+	{}
+
+	@(cpp_class)(@(native_type) msg) :
+		PX4Message<@(native_type)>(msg)
+	{}
+
+	~@(cpp_class)() {}
+
+	static PX4TopicHandle handle() {return "@(topic_name)";}
+};
+
+};
diff --git a/msg/templates/px4/msg.h.template b/msg/templates/px4/uorb/msg.h.template
similarity index 94%
rename from msg/templates/px4/msg.h.template
rename to msg/templates/px4/uorb/msg.h.template
index ba0fbd4bfe..2d42511074 100644
--- a/msg/templates/px4/msg.h.template
+++ b/msg/templates/px4/uorb/msg.h.template
@@ -56,7 +56,7 @@ import genmsg.msgs
 import gencpp
 
 cpp_class = 'px4_%s'%spec.short_name
-uorb_struct = '%s_s'%spec.short_name
+native_type = '%s_s'%spec.short_name
 topic_name = spec.short_name
 }@
 
@@ -76,15 +76,15 @@ namespace px4
 {
 
 class __EXPORT @(cpp_class) :
-	public PX4Message<@(uorb_struct)>
+	public PX4Message<@(native_type)>
 {
 public:
 	@(cpp_class)() :
-		PX4Message<@(uorb_struct)>()
+		PX4Message<@(native_type)>()
 	{}
 
-	@(cpp_class)(@(uorb_struct) msg) :
-		PX4Message<@(uorb_struct)>(msg)
+	@(cpp_class)(@(native_type) msg) :
+		PX4Message<@(native_type)>(msg)
 	{}
 
 	~@(cpp_class)() {}
diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h
deleted file mode 100644
index 5e9dc72cda..0000000000
--- a/src/platforms/ros/px4_messages/px4_rc_channels.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "px4/rc_channels.h"
-#include "platforms/px4_message.h"
-
-#pragma once
-namespace px4
-{
-
-class  px4_rc_channels :
-	public PX4Message<rc_channels>
-{
-public:
-	px4_rc_channels() :
-		PX4Message<rc_channels>()
-	{}
-
-	px4_rc_channels(rc_channels msg) :
-		PX4Message<rc_channels>(msg)
-	{}
-
-	~px4_rc_channels() {}
-
-	static PX4TopicHandle handle() {return "rc_channels";}
-};
-
-}

From f55832a37066c2cb0e6db64d95710b6fa0017fb1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 14:38:39 +0100
Subject: [PATCH 342/416] multiplatform: update topic includes

---
 src/platforms/px4_includes.h | 49 ++++++++++++++++++------------------
 1 file changed, 24 insertions(+), 25 deletions(-)

diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index bdb2b8de9b..90ec789960 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -48,21 +48,20 @@
 
 #ifdef __cplusplus
 #include "ros/ros.h"
-#include "px4/rc_channels.h"
-#include "px4_rc_channels.h" //XXX
-#include "px4/vehicle_attitude.h"
-#include <px4/vehicle_attitude_setpoint.h>
-#include <px4/manual_control_setpoint.h>
-#include <px4/actuator_controls.h>
-#include <px4/actuator_controls_0.h>
-#include <px4/actuator_controls_virtual_mc.h>
-#include <px4/vehicle_rates_setpoint.h>
-#include <px4/mc_virtual_rates_setpoint.h>
-#include <px4/vehicle_attitude.h>
-#include <px4/vehicle_control_mode.h>
-#include <px4/actuator_armed.h>
-#include <px4/parameter_update.h>
-#include <px4/vehicle_status.h>
+#include <px4_rc_channels.h>
+#include <px4_vehicle_attitude.h>
+#include <px4_vehicle_attitude_setpoint.h>
+#include <px4_manual_control_setpoint.h>
+#include <px4_actuator_controls.h>
+#include <px4_actuator_controls_0.h>
+#include <px4_actuator_controls_virtual_mc.h>
+#include <px4_vehicle_rates_setpoint.h>
+#include <px4_mc_virtual_rates_setpoint.h>
+#include <px4_vehicle_attitude.h>
+#include <px4_vehicle_control_mode.h>
+#include <px4_actuator_armed.h>
+#include <px4_parameter_update.h>
+#include <px4_vehicle_status.h>
 #endif
 
 #else
@@ -73,17 +72,17 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
 #ifdef __cplusplus
-#include <platforms/nuttx/px4_messages/px4_rc_channels.h> //XXX
+#include <platforms/nuttx/px4_messages/px4_rc_channels.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
+#include <platforms/nuttx/px4_messages/px4_parameter_update.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
 #endif
-#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/vehicle_control_mode.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>

From 1ab9ec5811a5b945ca49a8e6aeef59557dd2970c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 14:38:53 +0100
Subject: [PATCH 343/416] fix ros header template

---
 msg/templates/px4/ros/msg.h.template | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template
index 176be0d091..76abab163d 100644
--- a/msg/templates/px4/ros/msg.h.template
+++ b/msg/templates/px4/ros/msg.h.template
@@ -65,7 +65,7 @@ topic_name = spec.short_name
 @##############################
 @# Generic Includes
 @##############################
-#include <uORB/topics/@(topic_name).h>
+#include "px4/@(topic_name).h"
 #include "platforms/px4_message.h"
 
 @##############################

From 73f342616e275ca726d70807212a733f9971bf66 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 15:16:13 +0100
Subject: [PATCH 344/416] add better accessor

---
 src/examples/subscriber/subscriber_example.cpp |  2 +-
 src/platforms/px4_subscriber.h                 | 10 +++++++++-
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 215336c174..781dde486c 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -81,5 +81,5 @@ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
 	PX4_INFO("Callback (method): [%llu]",
 			msg.data().timestamp_last_valid);
 	PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]",
-			_sub_rc_chan->get().data().timestamp_last_valid);
+			_sub_rc_chan->data().timestamp_last_valid);
 }
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 6a81ef8d22..6ca35b173f 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -84,7 +84,15 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	virtual T get() {return _msg_current;}
+	virtual T& get() {return _msg_current;}
+
+	/**
+	 * Get the last native message value
+	 */
+	virtual decltype(((T*)nullptr)->data()) data()
+	{
+		return _msg_current.data();
+	}
 
 protected:
 	T _msg_current;				/**< Current Message value */

From 6c0b5cf1259a0560a5adc966c4ca306f998c2c86 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 23 Jan 2015 15:24:18 +0100
Subject: [PATCH 345/416] Update NuttX version as required

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index 3d8171f6ea..e4c914e261 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0
+Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67

From 6357fa75976a1d215d263b316d7fe495b82cfff4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 15:39:11 +0100
Subject: [PATCH 346/416] header generation script: create dir if it does not
 exist

---
 Tools/px_generate_uorb_topic_headers.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
index 54430cc38d..4bcab4d54c 100755
--- a/Tools/px_generate_uorb_topic_headers.py
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -90,6 +90,11 @@ def copy_changed(inputdir, outputdir, prefix=''):
         Copies files from inputdir to outputdir if they don't exist in
         ouputdir or if their content changed
         """
+
+        # Make sure output directory exists:
+        if not os.path.isdir(outputdir):
+                os.makedirs(outputdir)
+
         for f in os.listdir(inputdir):
                 fni = os.path.join(inputdir, f)
                 if os.path.isfile(fni):

From dfd078651f44533b2f5998b0edce781b11507275 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 18:00:59 +0100
Subject: [PATCH 347/416] subscriber: move nuttx startup handling

---
 src/examples/subscriber/module.mk             |  1 +
 src/examples/subscriber/subscriber_main.cpp   | 63 +-----------
 .../subscriber/subscriber_start_nuttx.cpp     | 99 +++++++++++++++++++
 3 files changed, 102 insertions(+), 61 deletions(-)
 create mode 100644 src/examples/subscriber/subscriber_start_nuttx.cpp

diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
index 3156ebb30a..0e02c65b45 100644
--- a/src/examples/subscriber/module.mk
+++ b/src/examples/subscriber/module.mk
@@ -38,6 +38,7 @@
 MODULE_COMMAND	= subscriber
 
 SRCS		= subscriber_main.cpp \
+		  subscriber_start_nuttx.cpp \
 		  subscriber_example.cpp \
 		  subscriber_params.c
 
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
index 906921e019..9752bc3cc8 100644
--- a/src/examples/subscriber/subscriber_main.cpp
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -37,70 +37,11 @@
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
-#include <string.h>
-#include <cstdlib>
 #include "subscriber_example.h"
+bool thread_running = false;     /**< Deamon status flag */
 
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-namespace px4
-{
-bool task_should_exit = false;
-}
-using namespace px4;
 
-PX4_MAIN_FUNCTION(subscriber);
-
-#if !defined(__PX4_ROS)
-extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
-int subscriber_main(int argc, char *argv[])
-{
-	if (argc < 1) {
-		errx(1, "usage: subscriber {start|stop|status}");
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		task_should_exit = false;
-
-		daemon_task = task_spawn_cmd("subscriber",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       subscriber_task_main,
-					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		task_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running");
-
-		} else {
-			warnx("not started");
-		}
-
-		exit(0);
-	}
-
-	warnx("unrecognized command");
-	return 1;
-}
-#endif
-
-PX4_MAIN_FUNCTION(subscriber)
+int main(int argc, char **argv)
 {
 	px4::init(argc, argv, "subscriber");
 
diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp
new file mode 100644
index 0000000000..6129b19acc
--- /dev/null
+++ b/src/examples/subscriber/subscriber_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
+int subscriber_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: subscriber {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("subscriber",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

From a36d0dce85868f35ad9c4e097f5b788e84d7d18f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 18:01:16 +0100
Subject: [PATCH 348/416] publisher: move nuttx startup handling

---
 src/examples/publisher/module.mk              |  1 +
 src/examples/publisher/publisher_main.cpp     | 61 +-----------
 .../publisher/publisher_start_nuttx.cpp       | 99 +++++++++++++++++++
 3 files changed, 102 insertions(+), 59 deletions(-)
 create mode 100644 src/examples/publisher/publisher_start_nuttx.cpp

diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk
index 4f941cd439..62a5f8dd1f 100644
--- a/src/examples/publisher/module.mk
+++ b/src/examples/publisher/module.mk
@@ -38,6 +38,7 @@
 MODULE_COMMAND	= publisher
 
 SRCS		= publisher_main.cpp \
+		  publisher_start_nuttx.cpp \
 		  publisher_example.cpp
 
 MODULE_STACKSIZE = 1200
diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index 81439a8be2..85d24d4ef2 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -41,66 +41,9 @@
 #include <cstdlib>
 #include "publisher_example.h"
 
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-namespace px4
-{
-bool task_should_exit = false;
-}
-using namespace px4;
+bool thread_running = false;     /**< Deamon status flag */
 
-PX4_MAIN_FUNCTION(publisher);
-
-#if !defined(__PX4_ROS)
-extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
-int publisher_main(int argc, char *argv[])
-{
-	if (argc < 1) {
-		errx(1, "usage: publisher {start|stop|status}");
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		task_should_exit = false;
-
-		daemon_task = task_spawn_cmd("publisher",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       publisher_task_main,
-					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		task_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running");
-
-		} else {
-			warnx("not started");
-		}
-
-		exit(0);
-	}
-
-	warnx("unrecognized command");
-	return 1;
-}
-#endif
-
-PX4_MAIN_FUNCTION(publisher)
+int main(int argc, char **argv)
 {
 	px4::init(argc, argv, "publisher");
 
diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp
new file mode 100644
index 0000000000..db9d85269b
--- /dev/null
+++ b/src/examples/publisher/publisher_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
+int publisher_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: publisher {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("publisher",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

From aa7e5ddb388e5d382b3dc214de6a1581c2a2fe71 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 18:11:44 +0100
Subject: [PATCH 349/416] move position of spin_once in publisher example

---
 src/examples/publisher/publisher_example.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index 6b30c2fe3d..9952a16e4b 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -54,13 +54,14 @@ int PublisherExample::main()
 	px4::Rate loop_rate(10);
 
 	while (px4::ok()) {
+		loop_rate.sleep();
+		_n.spinOnce();
+
 		px4_rc_channels msg;
 		msg.data().timestamp_last_valid = px4::get_time_micros();
 		PX4_INFO("%llu", msg.data().timestamp_last_valid);
 		_rc_channels_pub->publish(msg);
 
-		_n.spinOnce();
-		loop_rate.sleep();
 	}
 
 	return 0;

From 67a35eb0b509b2394f82773c460a6492471efd37 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 18:15:55 +0100
Subject: [PATCH 350/416] remove unneeded files

---
 src/platforms/nuttx/px4_nodehandle.cpp | 44 --------------------------
 src/platforms/nuttx/px4_publisher.cpp  | 41 ------------------------
 src/platforms/nuttx/px4_subscriber.cpp | 40 -----------------------
 src/platforms/ros/px4_subscriber.cpp   | 41 ------------------------
 4 files changed, 166 deletions(-)
 delete mode 100644 src/platforms/nuttx/px4_nodehandle.cpp
 delete mode 100644 src/platforms/nuttx/px4_publisher.cpp
 delete mode 100644 src/platforms/nuttx/px4_subscriber.cpp
 delete mode 100644 src/platforms/ros/px4_subscriber.cpp

diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
deleted file mode 100644
index ec557e8aa9..0000000000
--- a/src/platforms/nuttx/px4_nodehandle.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4_nodehandle.cpp
- *
- * PX4 Middleware Wrapper Nodehandle
- */
-#include <px4.h>
-
-namespace px4
-{
-
-}
diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp
deleted file mode 100644
index 3bd70272f1..0000000000
--- a/src/platforms/nuttx/px4_publisher.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4_publisher.cpp
- *
- * PX4 Middleware Wrapper for Publisher
- */
-#include <platforms/px4_publisher.h>
-
-
diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp
deleted file mode 100644
index 426e646c9f..0000000000
--- a/src/platforms/nuttx/px4_subscriber.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4_subscriber.cpp
- *
- * PX4 Middleware Wrapper Subscriber
- */
-#include <platforms/px4_subscriber.h>
-
diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp
deleted file mode 100644
index d040b860d2..0000000000
--- a/src/platforms/ros/px4_subscriber.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4_subscriber.cpp
- *
- * PX4 Middleware Wrapper Subscriber
- */
-
-#include <px4_subscriber.h>
-

From 4c3a24ddeeee4187a527a1d750b406b96232f66b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 23 Jan 2015 18:30:02 +0100
Subject: [PATCH 351/416] tiny cleanup

---
 src/examples/publisher/publisher_main.cpp   | 2 --
 src/examples/subscriber/subscriber_main.cpp | 1 -
 2 files changed, 3 deletions(-)

diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
index 85d24d4ef2..11439acffc 100644
--- a/src/examples/publisher/publisher_main.cpp
+++ b/src/examples/publisher/publisher_main.cpp
@@ -37,8 +37,6 @@
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
-#include <string.h>
-#include <cstdlib>
 #include "publisher_example.h"
 
 bool thread_running = false;     /**< Deamon status flag */
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
index 9752bc3cc8..798c741637 100644
--- a/src/examples/subscriber/subscriber_main.cpp
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -40,7 +40,6 @@
 #include "subscriber_example.h"
 bool thread_running = false;     /**< Deamon status flag */
 
-
 int main(int argc, char **argv)
 {
 	px4::init(argc, argv, "subscriber");

From 2d65e8cb3b792ae55c953df87fc429cdde2972b5 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 20 Jan 2015 16:50:59 +0100
Subject: [PATCH 352/416] initial commit for vagrant/docker based ros setup

---
 .gitignore                                    |  1 +
 Tools/ros/docker/px4-ros/Dockerfile           | 42 +++++++++++++
 .../docker/px4-ros/px4-ros-sitl/Dockerfile    | 22 +++++++
 .../px4-ros/px4-ros-sitl/get-sources.sh       | 28 +++++++++
 .../ros/vagrant/docker-host-base/Vagrantfile  | 59 +++++++++++++++++++
 .../docker-host-base/config/docker-default    | 26 ++++++++
 .../docker-host-base/config/xsessionrc        |  5 ++
 Tools/ros/vagrant/docker-host/Vagrantfile     | 25 ++++++++
 Tools/ros/vagrant/px4-ros-sitl/Vagrantfile    | 27 +++++++++
 9 files changed, 235 insertions(+)
 create mode 100644 Tools/ros/docker/px4-ros/Dockerfile
 create mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile
 create mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh
 create mode 100644 Tools/ros/vagrant/docker-host-base/Vagrantfile
 create mode 100644 Tools/ros/vagrant/docker-host-base/config/docker-default
 create mode 100644 Tools/ros/vagrant/docker-host-base/config/xsessionrc
 create mode 100644 Tools/ros/vagrant/docker-host/Vagrantfile
 create mode 100644 Tools/ros/vagrant/px4-ros-sitl/Vagrantfile

diff --git a/.gitignore b/.gitignore
index 1c4f49fdea..d4e2a46379 100644
--- a/.gitignore
+++ b/.gitignore
@@ -43,3 +43,4 @@ src/modules/uORB/topics/*
 Firmware.zip
 unittests/build
 *.generated.h
+.vagrant
diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile
new file mode 100644
index 0000000000..532a9af8b3
--- /dev/null
+++ b/Tools/ros/docker/px4-ros/Dockerfile
@@ -0,0 +1,42 @@
+#
+# Base ROS Indigo
+#
+
+FROM ubuntu:14.04.1
+MAINTAINER Andreas Antener "andreas@antener.name"
+
+# Install basics
+RUN apt-get update \
+	&& apt-get -y install wget git mercurial
+
+# Main ROS Setup
+# Following http://wiki.ros.org/indigo/Installation/Ubuntu
+# Also adding dependencies for gazebo http://gazebosim.org/tutorials?tut=drcsim_install
+
+## add ROS repositories and keys
+## install main ROS pacakges
+RUN echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list \
+	&& wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | apt-key add - \
+	&& apt-get update \
+	&& apt-get -y install ros-indigo-desktop-full
+
+RUN rosdep init \
+	&& rosdep update
+
+## setup environment variables
+RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
+
+## get rosinstall
+RUN apt-get -y install python-rosinstall
+
+## additional dependencies
+RUN apt-get -y install ros-indigo-octomap-msgs
+
+
+## install drcsim
+#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \
+#	&& wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \
+#	&& apt-get update \
+#	&& apt-get -y install drcsim
+
+CMD /bin/bash
diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile b/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile
new file mode 100644
index 0000000000..4c47295a8f
--- /dev/null
+++ b/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile
@@ -0,0 +1,22 @@
+#
+# PX4 ROS SITL
+#
+
+FROM px4ros/ros-base:no-drcsim
+MAINTAINER Andreas Antener "andreas@antener.name"
+
+# TODO
+#USER px4
+
+RUN . /opt/ros/indigo/setup.sh \
+	&& mkdir -p /catkin_ws/src \
+	&& cd /catkin_ws/src \
+	&& catkin_init_workspace \
+	&& cd /catkin_ws \
+	&& catkin_make \
+	&& echo "source /catkin_ws/devel/setup.bash" >> ~/.bashrc
+
+COPY get-sources.sh /catkin_ws/src/
+RUN chmod +x /catkin_ws/src/get-sources.sh
+
+CMD /bin/bash
diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh b/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh
new file mode 100644
index 0000000000..237ae7500a
--- /dev/null
+++ b/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh
@@ -0,0 +1,28 @@
+#!/bin/sh
+#
+# Fetch source repositories
+#
+
+# PX4 Firmware
+git clone https://github.com/PX4/Firmware.git \
+	&& cd Firmware \
+	&& git checkout ros \
+	&& cd ..
+
+# euroc simulator
+git clone https://github.com/PX4/euroc_simulator.git \
+	&& cd euroc_simulator \
+	&& git checkout px4_nodes \
+	&& cd ..
+
+# mav comm
+git clone https://github.com/PX4/mav_comm.git
+
+# glog catkin
+git clone https://github.com/ethz-asl/glog_catkin.git
+
+# catkin simple
+git clone https://github.com/catkin/catkin_simple.git
+
+echo "Execute catkin_make to compile all the sources."
+
diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
new file mode 100644
index 0000000000..5897735d0f
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -0,0 +1,59 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+#
+# Vagrantfile to create docker-host-base
+#
+# After build, do "vagrant package --base docker-host-base" to package,
+# and import as box: "vagrant box add --name docker-host-base package.box"
+#
+# To add local docker images into the docker host, configure your local
+# docker client to control the docker daemon on the later running docker-host VM.
+# This box configures docker to listen on any IP on port 2375.
+# You can then also load an existing image, e.g.:
+# "docker load -i px4ros_ros-sitl_no-drcsim_a4209708a04a.tar"
+#
+Vagrant.configure(2) do |config|
+  config.vm.box = "ubuntu/trusty64"
+
+  config.vm.define "docker-host-base"
+
+  config.vm.provider "virtualbox" do |vb|
+    vb.name = "docker-host-base"
+    vb.gui = true
+    vb.memory = "1024"
+  end
+  
+  config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default"
+  config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc"
+
+  config.vm.provision "shell", inline: <<-SHELL
+    # Update and install apps
+    sudo apt-get update
+    sudo apt-get upgrade -y
+    sudo apt-get install -y --no-install-recommends ubuntu-desktop
+    sudo apt-get install -y gnome-terminal unity-lens-applications
+
+    # Reset the ssh key (because vagrant regenerates it during provisioning)
+    sudo wget --no-check-certificate https://raw.github.com/mitchellh/vagrant/master/keys/vagrant.pub -O /home/vagrant/.ssh/authorized_keys
+    sudo chmod 0700 /home/vagrant/.ssh
+    sudo chmod 0600 /home/vagrant/.ssh/authorized_keys
+    sudo chown -R vagrant /home/vagrant/.ssh
+
+    # Copy docker config
+    sudo mv /home/vagrant/docker-default /etc/default/docker
+
+    # Enable autologin so docker can start GUI apps
+    sudo echo "autologin-user=vagrant" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf
+    sudo echo "autologin-user-timeout=0" >> /usr/share/lightdm/lightdm.conf.d/50-unity-greeter.conf
+
+    # X session RC
+    chmod +x /home/vagrant/.xsessionrc
+  SHELL
+
+  config.vm.provision "docker"
+
+  # Shutdown after provisioning. "vagrant halt" doesn't recognize the original ssh key anymore
+  # and would just kill the VM. This might lead to FS inconsistencies (e.g. in the docker DB).
+  config.vm.provision "shell", inline: "sudo shutdown -h now"
+end
diff --git a/Tools/ros/vagrant/docker-host-base/config/docker-default b/Tools/ros/vagrant/docker-host-base/config/docker-default
new file mode 100644
index 0000000000..e8a86ce6a1
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host-base/config/docker-default
@@ -0,0 +1,26 @@
+#
+# Default config for docker /etc/default/docker
+# Copied from a provisioned vagrant box
+#
+# Modifications:
+# - listen to TCP port
+# - removing deprecated "-r=true" option which apparently doesn't work anymore
+#   > use restart policies for specific containers if necessary
+#
+
+# Docker Upstart and SysVinit configuration file
+
+# Customize location of Docker binary (especially for development testing).
+#DOCKER="/usr/local/bin/docker"
+
+# Use DOCKER_OPTS to modify the daemon startup options.
+#DOCKER_OPTS="--dns 8.8.8.8 --dns 8.8.4.4"
+
+# If you need Docker to use an HTTP proxy, it can also be specified here.
+#export http_proxy="http://127.0.0.1:3128/"
+
+# This is also a handy place to tweak where Docker's temporary files go.
+#export TMPDIR="/mnt/bigdrive/docker-tmp"
+
+# Expose TCP port in addition to socket
+DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375"
diff --git a/Tools/ros/vagrant/docker-host-base/config/xsessionrc b/Tools/ros/vagrant/docker-host-base/config/xsessionrc
new file mode 100644
index 0000000000..d8ab6d53da
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host-base/config/xsessionrc
@@ -0,0 +1,5 @@
+#!/bin/sh
+#
+# Disable X access control so we can easily start GUI apps
+#
+xhost +
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
new file mode 100644
index 0000000000..2e5fcb774e
--- /dev/null
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -0,0 +1,25 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+Vagrant.configure(2) do |config|
+  config.vm.box = "docker-host-base"
+
+  config.vm.define "docker-host"
+
+  config.vm.provider "virtualbox" do |vb|
+    vb.name = "docker-host"
+    vb.gui = true
+    vb.memory = "4096"
+    vb.cpus = 2
+    vb.customize ["modifyvm", :id, "--graphicscontroller", "vboxvga"]
+    vb.customize ["modifyvm", :id, "--accelerate3d", "on"]
+    vb.customize ["modifyvm", :id, "--ioapic", "on"]
+    vb.customize ["modifyvm", :id, "--vram", "128"]
+    vb.customize ["modifyvm", :id, "--hwvirtex", "on"]
+  end
+
+  config.vm.network "private_network", ip: "192.168.59.104"
+
+  # TBD: would it be better to provision docker here instead of in the base box?
+  #config.vm.provision "docker"
+end
diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
new file mode 100644
index 0000000000..5357ce94dc
--- /dev/null
+++ b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
@@ -0,0 +1,27 @@
+# -*- mode: ruby -*-
+# vi: set ft=ruby :
+
+Vagrant.configure(2) do |config|
+  # Configure docker host
+  config.vm.provider "docker" do |d|
+    d.vagrant_machine = "docker-host"
+    d.vagrant_vagrantfile = "../docker-host/Vagrantfile"
+  end
+
+  # Configure docker apps to run
+  config.vm.define "gazebo" do |app|
+    app.vm.provider "docker" do |d|
+      d.name = "gazebo"
+      d.image = "px4ros/ros-sitl:no-drcsim"
+
+      # share docker host x11 socket
+      d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"]
+      # TODO: get display number from host system
+      d.env = {
+        "DISPLAY" => ":0"
+      }
+
+      d.cmd = ["xterm"]
+    end
+  end
+end

From 05589e40dfb39e83681ac909545d20bdb96ca773 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 21 Jan 2015 00:01:55 +0100
Subject: [PATCH 353/416] updated docker instrumentalization and info

---
 Tools/ros/vagrant/px4-ros-sitl/Vagrantfile | 37 +++++++++++++++++++---
 1 file changed, 33 insertions(+), 4 deletions(-)

diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
index 5357ce94dc..5edf8f0acd 100644
--- a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
@@ -1,6 +1,14 @@
 # -*- mode: ruby -*-
 # vi: set ft=ruby :
 
+#
+# Boot docker SITL environment
+#
+# Build (first time up):
+# Use the "--no-parallel" option so the containers will be built in order.
+# e.g.: "vagrant up --no-parallel"
+#
+
 Vagrant.configure(2) do |config|
   # Configure docker host
   config.vm.provider "docker" do |d|
@@ -9,10 +17,12 @@ Vagrant.configure(2) do |config|
   end
 
   # Configure docker apps to run
-  config.vm.define "gazebo" do |app|
+  config.vm.define "ros" do |app|
     app.vm.provider "docker" do |d|
-      d.name = "gazebo"
-      d.image = "px4ros/ros-sitl:no-drcsim"
+      d.name = "ros"
+      #d.image = "px4ros/ros-base:no-drcsim"
+      d.build_dir = "../../docker/px4-ros"
+      d.build_args = ["-t=px4ros/ros-base:no-drcsim"]
 
       # share docker host x11 socket
       d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"]
@@ -21,7 +31,26 @@ Vagrant.configure(2) do |config|
         "DISPLAY" => ":0"
       }
 
-      d.cmd = ["xterm"]
+      d.cmd = ["echo", "Base image done"]
+      d.remains_running = false
+    end
+  end
+
+  config.vm.define "gazebo" do |app|
+    app.vm.provider "docker" do |d|
+      d.name = "gazebo"
+      #d.image = "px4ros/ros-sitl"
+      d.build_dir = "../../docker/px4-ros/px4-ros-sitl"
+      d.build_args = ["-t=px4ros/ros-sitl:no-drcsim"]
+
+      # share docker host x11 socket
+      d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"]
+      # TODO: get display number from host system
+      d.env = {
+        "DISPLAY" => ":0"
+      }
+
+      d.cmd = ["gazebo"]
     end
   end
 end

From 0bfeedef89785646bbb05fd5583f18f42c905770 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 21 Jan 2015 14:54:55 +0100
Subject: [PATCH 354/416] updated build and notes

---
 Tools/ros/docker/px4-ros/Dockerfile           | 16 +++++++-
 .../ros/vagrant/docker-host-base/Vagrantfile  |  1 +
 Tools/ros/vagrant/docker-host/Vagrantfile     |  3 ++
 Tools/ros/vagrant/px4-ros-sitl/Vagrantfile    | 38 +++++++++----------
 4 files changed, 35 insertions(+), 23 deletions(-)

diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile
index 532a9af8b3..668d507997 100644
--- a/Tools/ros/docker/px4-ros/Dockerfile
+++ b/Tools/ros/docker/px4-ros/Dockerfile
@@ -6,8 +6,11 @@ FROM ubuntu:14.04.1
 MAINTAINER Andreas Antener "andreas@antener.name"
 
 # Install basics
+## Use the "noninteractive" debconf frontend
+ENV DEBIAN_FRONTEND noninteractive
+
 RUN apt-get update \
-	&& apt-get -y install wget git mercurial
+	&& apt-get -y install wget git mercurial openssh-server
 
 # Main ROS Setup
 # Following http://wiki.ros.org/indigo/Installation/Ubuntu
@@ -39,4 +42,13 @@ RUN apt-get -y install ros-indigo-octomap-msgs
 #	&& apt-get update \
 #	&& apt-get -y install drcsim
 
-CMD /bin/bash
+# some QT-Apps/Gazebo don't not show controls without this
+ENV QT_X11_NO_MITSHM 1
+
+#RUN apt-get install -y openssh-server
+# Install x11-utils to get xdpyinfo, for X11 display debugging
+# mesa-utils provides glxinfo, handy for understanding the 3D support.
+#RUN apt-get -y install x11-utils
+#RUN apt-get -y install mesa-utils
+
+#CMD ["/bin/sbin/sshd", "-D"]
diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
index 5897735d0f..516be528cd 100644
--- a/Tools/ros/vagrant/docker-host-base/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -29,6 +29,7 @@ Vagrant.configure(2) do |config|
 
   config.vm.provision "shell", inline: <<-SHELL
     # Update and install apps
+    export DEBIAN_FRONTEND=noninteractive
     sudo apt-get update
     sudo apt-get upgrade -y
     sudo apt-get install -y --no-install-recommends ubuntu-desktop
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
index 2e5fcb774e..42e63567ae 100644
--- a/Tools/ros/vagrant/docker-host/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -1,6 +1,9 @@
 # -*- mode: ruby -*-
 # vi: set ft=ruby :
 
+#
+# Actual docker host VM to run.
+#
 Vagrant.configure(2) do |config|
   config.vm.box = "docker-host-base"
 
diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
index 5edf8f0acd..d670e62509 100644
--- a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
@@ -4,10 +4,19 @@
 #
 # Boot docker SITL environment
 #
-# Build (first time up):
+# "vagrant up" will build the images. Should start "xterm" in the end.
+#
+# Notes
+# (will change, need proper docs)
+#
+# Build with multiple dependent docker containers:
 # Use the "--no-parallel" option so the containers will be built in order.
 # e.g.: "vagrant up --no-parallel"
 #
+# Running apps directly:
+# "vagrant docker-run ros -- <cmd>"
+# Attention: will loose all data when stopped, vagrant runs this with "--rm"
+#
 
 Vagrant.configure(2) do |config|
   # Configure docker host
@@ -25,32 +34,19 @@ Vagrant.configure(2) do |config|
       d.build_args = ["-t=px4ros/ros-base:no-drcsim"]
 
       # share docker host x11 socket
-      d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"]
+      d.volumes = [
+        "/tmp/.X11-unix:/tmp/.X11-unix:ro",
+        "/dev/dri:/dev/dri"
+      ]
       # TODO: get display number from host system
       d.env = {
         "DISPLAY" => ":0"
       }
 
-      d.cmd = ["echo", "Base image done"]
-      d.remains_running = false
+      d.remains_running = true
+      d.cmd = ["xterm"]
+      #d.has_ssh = true
     end
   end
 
-  config.vm.define "gazebo" do |app|
-    app.vm.provider "docker" do |d|
-      d.name = "gazebo"
-      #d.image = "px4ros/ros-sitl"
-      d.build_dir = "../../docker/px4-ros/px4-ros-sitl"
-      d.build_args = ["-t=px4ros/ros-sitl:no-drcsim"]
-
-      # share docker host x11 socket
-      d.volumes = ["/tmp/.X11-unix:/tmp/.X11-unix:ro"]
-      # TODO: get display number from host system
-      d.env = {
-        "DISPLAY" => ":0"
-      }
-
-      d.cmd = ["gazebo"]
-    end
-  end
 end

From 353c230db5927b3e060bbfde9a0aa34e27abbe35 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 22 Jan 2015 17:31:47 +0100
Subject: [PATCH 355/416] restructured things a little bit

---
 Tools/ros/docker/px4-ros/Dockerfile           | 22 ++++++++++------
 .../docker/px4-ros/px4-ros-sitl/Dockerfile    | 22 ----------------
 .../setup-workspace.sh}                       | 25 +++++++++++++++----
 .../{px4-ros-sitl => px4-ros}/Vagrantfile     |  5 +++-
 4 files changed, 39 insertions(+), 35 deletions(-)
 delete mode 100644 Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile
 rename Tools/ros/docker/px4-ros/{px4-ros-sitl/get-sources.sh => scripts/setup-workspace.sh} (51%)
 rename Tools/ros/vagrant/{px4-ros-sitl => px4-ros}/Vagrantfile (91%)

diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile
index 668d507997..74291bd14b 100644
--- a/Tools/ros/docker/px4-ros/Dockerfile
+++ b/Tools/ros/docker/px4-ros/Dockerfile
@@ -1,16 +1,21 @@
 #
 # Base ROS Indigo
 #
+# TODO
+# - use https://github.com/phusion/baseimage-docker as base
+# - add user, best synced with host
+# - configure ssh to work with vagrant out of the box
+#
 
 FROM ubuntu:14.04.1
-MAINTAINER Andreas Antener "andreas@antener.name"
+MAINTAINER Andreas Antener "andreas@uaventure.com"
 
 # Install basics
 ## Use the "noninteractive" debconf frontend
 ENV DEBIAN_FRONTEND noninteractive
 
 RUN apt-get update \
-	&& apt-get -y install wget git mercurial openssh-server
+	&& apt-get -y install wget git mercurial
 
 # Main ROS Setup
 # Following http://wiki.ros.org/indigo/Installation/Ubuntu
@@ -42,13 +47,16 @@ RUN apt-get -y install ros-indigo-octomap-msgs
 #	&& apt-get update \
 #	&& apt-get -y install drcsim
 
-# some QT-Apps/Gazebo don't not show controls without this
-ENV QT_X11_NO_MITSHM 1
-
-#RUN apt-get install -y openssh-server
 # Install x11-utils to get xdpyinfo, for X11 display debugging
 # mesa-utils provides glxinfo, handy for understanding the 3D support.
 #RUN apt-get -y install x11-utils
 #RUN apt-get -y install mesa-utils
 
-#CMD ["/bin/sbin/sshd", "-D"]
+# Some QT-Apps/Gazebo don't not show controls without this
+ENV QT_X11_NO_MITSHM 1
+
+COPY scripts/setup-workspace.sh ~/
+RUN chmod +x ~/setup-workspace.sh
+
+
+CMD ["/usr/bin/xterm"]
diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile b/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile
deleted file mode 100644
index 4c47295a8f..0000000000
--- a/Tools/ros/docker/px4-ros/px4-ros-sitl/Dockerfile
+++ /dev/null
@@ -1,22 +0,0 @@
-#
-# PX4 ROS SITL
-#
-
-FROM px4ros/ros-base:no-drcsim
-MAINTAINER Andreas Antener "andreas@antener.name"
-
-# TODO
-#USER px4
-
-RUN . /opt/ros/indigo/setup.sh \
-	&& mkdir -p /catkin_ws/src \
-	&& cd /catkin_ws/src \
-	&& catkin_init_workspace \
-	&& cd /catkin_ws \
-	&& catkin_make \
-	&& echo "source /catkin_ws/devel/setup.bash" >> ~/.bashrc
-
-COPY get-sources.sh /catkin_ws/src/
-RUN chmod +x /catkin_ws/src/get-sources.sh
-
-CMD /bin/bash
diff --git a/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
similarity index 51%
rename from Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh
rename to Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
index 237ae7500a..6b5ddb0a55 100644
--- a/Tools/ros/docker/px4-ros/px4-ros-sitl/get-sources.sh
+++ b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
@@ -1,28 +1,43 @@
 #!/bin/sh
 #
-# Fetch source repositories
+# Create workspace at current location and fetch source repositories
 #
 
+WDIR=`pwd`
+WORKSPACE=$WDIR/catkin_ws
+
+# Setup workspace
+mkdir -p $WORKSPACE/src
+cd $WORKSPACE/src
+catkin_init_workspace
+cd $WORKSPACE
+catkin_make
+echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc
+
 # PX4 Firmware
+cd $WORKSPACE/src
 git clone https://github.com/PX4/Firmware.git \
 	&& cd Firmware \
-	&& git checkout ros \
-	&& cd ..
+	&& git checkout ros
 
 # euroc simulator
+cd $WORKSPACE/src
 git clone https://github.com/PX4/euroc_simulator.git \
 	&& cd euroc_simulator \
-	&& git checkout px4_nodes \
-	&& cd ..
+	&& git checkout px4_nodes
 
 # mav comm
+cd $WORKSPACE/src
 git clone https://github.com/PX4/mav_comm.git
 
 # glog catkin
+cd $WORKSPACE/src
 git clone https://github.com/ethz-asl/glog_catkin.git
 
 # catkin simple
+cd $WORKSPACE/src
 git clone https://github.com/catkin/catkin_simple.git
 
+cd $WORKSPACE
 echo "Execute catkin_make to compile all the sources."
 
diff --git a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
similarity index 91%
rename from Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
rename to Tools/ros/vagrant/px4-ros/Vagrantfile
index d670e62509..b5169e061a 100644
--- a/Tools/ros/vagrant/px4-ros-sitl/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -6,7 +6,7 @@
 #
 # "vagrant up" will build the images. Should start "xterm" in the end.
 #
-# Notes
+# Notes:
 # (will change, need proper docs)
 #
 # Build with multiple dependent docker containers:
@@ -17,6 +17,9 @@
 # "vagrant docker-run ros -- <cmd>"
 # Attention: will loose all data when stopped, vagrant runs this with "--rm"
 #
+# TODO
+# - maybe map a local working directory to compile stuff without loosing it in side the docker container
+#
 
 Vagrant.configure(2) do |config|
   # Configure docker host

From da8c9af37b2aa183499fb8e2bb205633bce92ded Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 22 Jan 2015 18:30:15 +0100
Subject: [PATCH 356/416] fixed some stuff

---
 Tools/ros/docker/px4-ros/Dockerfile                 | 4 ++--
 Tools/ros/docker/px4-ros/scripts/setup-workspace.sh | 2 +-
 Tools/ros/vagrant/px4-ros/Vagrantfile               | 1 +
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile
index 74291bd14b..4b553fa66e 100644
--- a/Tools/ros/docker/px4-ros/Dockerfile
+++ b/Tools/ros/docker/px4-ros/Dockerfile
@@ -55,8 +55,8 @@ RUN apt-get -y install ros-indigo-octomap-msgs
 # Some QT-Apps/Gazebo don't not show controls without this
 ENV QT_X11_NO_MITSHM 1
 
-COPY scripts/setup-workspace.sh ~/
-RUN chmod +x ~/setup-workspace.sh
+COPY scripts/setup-workspace.sh /root/scripts
+RUN chmod +x /root/setup-workspace.sh
 
 
 CMD ["/usr/bin/xterm"]
diff --git a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
index 6b5ddb0a55..231166e275 100644
--- a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
+++ b/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
@@ -39,5 +39,5 @@ cd $WORKSPACE/src
 git clone https://github.com/catkin/catkin_simple.git
 
 cd $WORKSPACE
-echo "Execute catkin_make to compile all the sources."
+catkin_make
 
diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
index b5169e061a..e54370ada5 100644
--- a/Tools/ros/vagrant/px4-ros/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -18,6 +18,7 @@
 # Attention: will loose all data when stopped, vagrant runs this with "--rm"
 #
 # TODO
+# - do not run the docker container with "--rm" (vagrant default). is that even possible?
 # - maybe map a local working directory to compile stuff without loosing it in side the docker container
 #
 

From 4fda482a7385e132df1064d6380b762c6b11710e Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 22 Jan 2015 18:53:36 +0100
Subject: [PATCH 357/416] updated copy instruction

---
 Tools/ros/docker/px4-ros/Dockerfile | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros/Dockerfile
index 4b553fa66e..d896c76e7e 100644
--- a/Tools/ros/docker/px4-ros/Dockerfile
+++ b/Tools/ros/docker/px4-ros/Dockerfile
@@ -55,8 +55,9 @@ RUN apt-get -y install ros-indigo-octomap-msgs
 # Some QT-Apps/Gazebo don't not show controls without this
 ENV QT_X11_NO_MITSHM 1
 
+# FIXME: this doesn't work yet when building from vagrant
 COPY scripts/setup-workspace.sh /root/scripts
-RUN chmod +x /root/setup-workspace.sh
+#RUN chmod +x /root/scripts/setup-workspace.sh
 
 
 CMD ["/usr/bin/xterm"]

From 6db56b8df08e6dfe4670e1e10a3fb391470f32b4 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 22 Jan 2015 11:24:46 -0800
Subject: [PATCH 358/416] added privileged options to support hardware
 acceleration

---
 Tools/ros/vagrant/px4-ros/Vagrantfile | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
index e54370ada5..ffe3a00ea0 100644
--- a/Tools/ros/vagrant/px4-ros/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -37,11 +37,13 @@ Vagrant.configure(2) do |config|
       d.build_dir = "../../docker/px4-ros"
       d.build_args = ["-t=px4ros/ros-base:no-drcsim"]
 
-      # share docker host x11 socket
+      # Share docker host x11 socket
+      # Run privileged to support 3d acceleration
       d.volumes = [
-        "/tmp/.X11-unix:/tmp/.X11-unix:ro",
-        "/dev/dri:/dev/dri"
+        "/tmp/.X11-unix:/tmp/.X11-unix:ro"
       ]
+      d.create_args = ["--privileged"]
+
       # TODO: get display number from host system
       d.env = {
         "DISPLAY" => ":0"

From 6ab1f2168059bb7689548635ab4e746c8f320295 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 22 Jan 2015 21:53:11 +0100
Subject: [PATCH 359/416] updated setup, added maintainer

---
 Tools/ros/docker/{px4-ros => px4-ros-base}/Dockerfile | 11 +++++------
 .../scripts/setup-workspace.sh                        |  0
 Tools/ros/vagrant/docker-host-base/Vagrantfile        |  2 ++
 Tools/ros/vagrant/docker-host/Vagrantfile             |  2 ++
 Tools/ros/vagrant/px4-ros/Vagrantfile                 |  8 +++++---
 5 files changed, 14 insertions(+), 9 deletions(-)
 rename Tools/ros/docker/{px4-ros => px4-ros-base}/Dockerfile (89%)
 rename Tools/ros/docker/{px4-ros => px4-ros-base}/scripts/setup-workspace.sh (100%)

diff --git a/Tools/ros/docker/px4-ros/Dockerfile b/Tools/ros/docker/px4-ros-base/Dockerfile
similarity index 89%
rename from Tools/ros/docker/px4-ros/Dockerfile
rename to Tools/ros/docker/px4-ros-base/Dockerfile
index d896c76e7e..a8c3772881 100644
--- a/Tools/ros/docker/px4-ros/Dockerfile
+++ b/Tools/ros/docker/px4-ros-base/Dockerfile
@@ -1,5 +1,5 @@
 #
-# Base ROS Indigo
+# PX4 ROS base container
 #
 # TODO
 # - use https://github.com/phusion/baseimage-docker as base
@@ -8,7 +8,7 @@
 #
 
 FROM ubuntu:14.04.1
-MAINTAINER Andreas Antener "andreas@uaventure.com"
+MAINTAINER Andreas Antener <andreas@uaventure.com>
 
 # Install basics
 ## Use the "noninteractive" debconf frontend
@@ -48,14 +48,13 @@ RUN apt-get -y install ros-indigo-octomap-msgs
 #	&& apt-get -y install drcsim
 
 # Install x11-utils to get xdpyinfo, for X11 display debugging
-# mesa-utils provides glxinfo, handy for understanding the 3D support.
-#RUN apt-get -y install x11-utils
-#RUN apt-get -y install mesa-utils
+# mesa-utils provides glxinfo, handy for understanding the 3D support
+RUN apt-get -y install x11-utils mesa-utils
 
 # Some QT-Apps/Gazebo don't not show controls without this
 ENV QT_X11_NO_MITSHM 1
 
-# FIXME: this doesn't work yet when building from vagrant
+# FIXME: this doesn't work when building from vagrant
 COPY scripts/setup-workspace.sh /root/scripts
 #RUN chmod +x /root/scripts/setup-workspace.sh
 
diff --git a/Tools/ros/docker/px4-ros/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh
similarity index 100%
rename from Tools/ros/docker/px4-ros/scripts/setup-workspace.sh
rename to Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh
diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
index 516be528cd..cb66d4a682 100644
--- a/Tools/ros/vagrant/docker-host-base/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -4,6 +4,8 @@
 #
 # Vagrantfile to create docker-host-base
 #
+# Maintainer: Andreas Antener <andreas@uaventure.com>
+#
 # After build, do "vagrant package --base docker-host-base" to package,
 # and import as box: "vagrant box add --name docker-host-base package.box"
 #
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
index 42e63567ae..9fd4695dd5 100644
--- a/Tools/ros/vagrant/docker-host/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -4,6 +4,8 @@
 #
 # Actual docker host VM to run.
 #
+# Maintainer: Andreas Antener <andreas@uaventure.com>
+#
 Vagrant.configure(2) do |config|
   config.vm.box = "docker-host-base"
 
diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
index ffe3a00ea0..7009fd78ac 100644
--- a/Tools/ros/vagrant/px4-ros/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -4,7 +4,9 @@
 #
 # Boot docker SITL environment
 #
-# "vagrant up" will build the images. Should start "xterm" in the end.
+# Maintainer: Andreas Antener <andreas@uaventure.com>
+#
+# "vagrant up" will build the images. Should eventually start "xterm" from within the docker container.
 #
 # Notes:
 # (will change, need proper docs)
@@ -33,8 +35,8 @@ Vagrant.configure(2) do |config|
   config.vm.define "ros" do |app|
     app.vm.provider "docker" do |d|
       d.name = "ros"
-      #d.image = "px4ros/ros-base:no-drcsim"
-      d.build_dir = "../../docker/px4-ros"
+      #d.image = "px4ros/ros-base"
+      d.build_dir = "../../docker/px4-ros-base"
       d.build_args = ["-t=px4ros/ros-base:no-drcsim"]
 
       # Share docker host x11 socket

From aae96a1b1631677dd07398e1134f77d4c5e38f08 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 23 Jan 2015 10:51:13 +0100
Subject: [PATCH 360/416] updated docs, fixed script copy, renamed stuff

---
 .../docker/{px4-ros-base => px4-ros-full}/Dockerfile   |  8 ++++----
 .../scripts/setup-workspace.sh                         |  0
 Tools/ros/vagrant/docker-host-base/Vagrantfile         |  6 ------
 Tools/ros/vagrant/docker-host/Vagrantfile              |  6 ++++++
 Tools/ros/vagrant/px4-ros/Vagrantfile                  | 10 +++++-----
 5 files changed, 15 insertions(+), 15 deletions(-)
 rename Tools/ros/docker/{px4-ros-base => px4-ros-full}/Dockerfile (92%)
 rename Tools/ros/docker/{px4-ros-base => px4-ros-full}/scripts/setup-workspace.sh (100%)

diff --git a/Tools/ros/docker/px4-ros-base/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile
similarity index 92%
rename from Tools/ros/docker/px4-ros-base/Dockerfile
rename to Tools/ros/docker/px4-ros-full/Dockerfile
index a8c3772881..be98cba583 100644
--- a/Tools/ros/docker/px4-ros-base/Dockerfile
+++ b/Tools/ros/docker/px4-ros-full/Dockerfile
@@ -1,5 +1,5 @@
 #
-# PX4 ROS base container
+# PX4 full ROS container
 #
 # TODO
 # - use https://github.com/phusion/baseimage-docker as base
@@ -55,8 +55,8 @@ RUN apt-get -y install x11-utils mesa-utils
 ENV QT_X11_NO_MITSHM 1
 
 # FIXME: this doesn't work when building from vagrant
-COPY scripts/setup-workspace.sh /root/scripts
-#RUN chmod +x /root/scripts/setup-workspace.sh
-
+COPY scripts/setup-workspace.sh /root/scripts/
+RUN chmod +x -R /root/scripts/*
+RUN chown -R root:root /root/scripts/*
 
 CMD ["/usr/bin/xterm"]
diff --git a/Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
similarity index 100%
rename from Tools/ros/docker/px4-ros-base/scripts/setup-workspace.sh
rename to Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
index cb66d4a682..196a8fb121 100644
--- a/Tools/ros/vagrant/docker-host-base/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -9,12 +9,6 @@
 # After build, do "vagrant package --base docker-host-base" to package,
 # and import as box: "vagrant box add --name docker-host-base package.box"
 #
-# To add local docker images into the docker host, configure your local
-# docker client to control the docker daemon on the later running docker-host VM.
-# This box configures docker to listen on any IP on port 2375.
-# You can then also load an existing image, e.g.:
-# "docker load -i px4ros_ros-sitl_no-drcsim_a4209708a04a.tar"
-#
 Vagrant.configure(2) do |config|
   config.vm.box = "ubuntu/trusty64"
 
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
index 9fd4695dd5..2b85bf8e95 100644
--- a/Tools/ros/vagrant/docker-host/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -6,6 +6,12 @@
 #
 # Maintainer: Andreas Antener <andreas@uaventure.com>
 #
+# To add local docker images into the docker host, configure your local
+# docker client to control the docker daemon on the running "docker-host" VM.
+# The box ("docker-host-base") configures docker to listen on any IP on port 2375.
+# You can then load an existing image, e.g.:
+# "docker load -i container-image.tar"
+#
 Vagrant.configure(2) do |config|
   config.vm.box = "docker-host-base"
 
diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
index 7009fd78ac..a122fb4a3b 100644
--- a/Tools/ros/vagrant/px4-ros/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -12,12 +12,12 @@
 # (will change, need proper docs)
 #
 # Build with multiple dependent docker containers:
-# Use the "--no-parallel" option so the containers will be built in order.
+# Use the "--no-parallel" option so the containers will be built/started in order.
 # e.g.: "vagrant up --no-parallel"
 #
 # Running apps directly:
 # "vagrant docker-run ros -- <cmd>"
-# Attention: will loose all data when stopped, vagrant runs this with "--rm"
+# Attention: will loose all data when stopped, vagrant runs docker always with "--rm"
 #
 # TODO
 # - do not run the docker container with "--rm" (vagrant default). is that even possible?
@@ -35,9 +35,9 @@ Vagrant.configure(2) do |config|
   config.vm.define "ros" do |app|
     app.vm.provider "docker" do |d|
       d.name = "ros"
-      #d.image = "px4ros/ros-base"
-      d.build_dir = "../../docker/px4-ros-base"
-      d.build_args = ["-t=px4ros/ros-base:no-drcsim"]
+      d.image = "uaventure/px4-ros-full"
+      #d.build_dir = "../../docker/px4-ros-full"
+      #d.build_args = ["-t=uaventure/px4-ros-full"]
 
       # Share docker host x11 socket
       # Run privileged to support 3d acceleration

From c4b309c06e0063559ac8c2fce736f3296875ea5a Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 23 Jan 2015 10:56:25 +0100
Subject: [PATCH 361/416] added readme for docker container

---
 Tools/ros/docker/px4-ros-full/Dockerfile |  5 -----
 Tools/ros/docker/px4-ros-full/README.md  | 10 ++++++++++
 2 files changed, 10 insertions(+), 5 deletions(-)
 create mode 100644 Tools/ros/docker/px4-ros-full/README.md

diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile
index be98cba583..c60f665396 100644
--- a/Tools/ros/docker/px4-ros-full/Dockerfile
+++ b/Tools/ros/docker/px4-ros-full/Dockerfile
@@ -1,11 +1,6 @@
 #
 # PX4 full ROS container
 #
-# TODO
-# - use https://github.com/phusion/baseimage-docker as base
-# - add user, best synced with host
-# - configure ssh to work with vagrant out of the box
-#
 
 FROM ubuntu:14.04.1
 MAINTAINER Andreas Antener <andreas@uaventure.com>
diff --git a/Tools/ros/docker/px4-ros-full/README.md b/Tools/ros/docker/px4-ros-full/README.md
new file mode 100644
index 0000000000..7d75754d9c
--- /dev/null
+++ b/Tools/ros/docker/px4-ros-full/README.md
@@ -0,0 +1,10 @@
+# PX4 ROS #
+
+Full desktop ROS container.
+
+**TODO:**
+
+- use https://github.com/phusion/baseimage-docker as base
+- add user, best synced with host
+- configure ssh to work with vagrant out of the box
+

From 700ca3c7d0b49b9c9f2403a630c3335c518ebddc Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 23 Jan 2015 11:53:54 +0100
Subject: [PATCH 362/416] naming updates

---
 Tools/ros/vagrant/docker-host-base/Vagrantfile | 2 +-
 Tools/ros/vagrant/docker-host/Vagrantfile      | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
index 196a8fb121..bc9329bf80 100644
--- a/Tools/ros/vagrant/docker-host-base/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -7,7 +7,7 @@
 # Maintainer: Andreas Antener <andreas@uaventure.com>
 #
 # After build, do "vagrant package --base docker-host-base" to package,
-# and import as box: "vagrant box add --name docker-host-base package.box"
+# and import as box: "vagrant box add --name uaventure/docker-host-base package.box"
 #
 Vagrant.configure(2) do |config|
   config.vm.box = "ubuntu/trusty64"
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
index 2b85bf8e95..4ae94258d3 100644
--- a/Tools/ros/vagrant/docker-host/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -13,7 +13,7 @@
 # "docker load -i container-image.tar"
 #
 Vagrant.configure(2) do |config|
-  config.vm.box = "docker-host-base"
+  config.vm.box = "uaventure/docker-host-base"
 
   config.vm.define "docker-host"
 

From 8194cb107169d7c1a642e7bbcbade92879da6c9b Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 23 Jan 2015 23:25:27 +0100
Subject: [PATCH 363/416] added ros-joy package and drcsim

---
 Tools/ros/docker/px4-ros-full/Dockerfile | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile
index c60f665396..9c0fbc6886 100644
--- a/Tools/ros/docker/px4-ros-full/Dockerfile
+++ b/Tools/ros/docker/px4-ros-full/Dockerfile
@@ -33,14 +33,13 @@ RUN echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
 RUN apt-get -y install python-rosinstall
 
 ## additional dependencies
-RUN apt-get -y install ros-indigo-octomap-msgs
-
+RUN apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy
 
 ## install drcsim
-#RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \
-#	&& wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \
-#	&& apt-get update \
-#	&& apt-get -y install drcsim
+RUN echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list \
+	&& wget http://packages.osrfoundation.org/drc.key -O - | apt-key add - \
+	&& apt-get update \
+	&& apt-get -y install drcsim
 
 # Install x11-utils to get xdpyinfo, for X11 display debugging
 # mesa-utils provides glxinfo, handy for understanding the 3D support

From 36a70debf4c51c7ad02651f2eb6a24da71bad2fc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 13:35:57 +0100
Subject: [PATCH 364/416] add act control 0 to 3 as msg

---
 msg/actuator_controls_1.msg | 5 +++++
 msg/actuator_controls_2.msg | 5 +++++
 msg/actuator_controls_3.msg | 5 +++++
 3 files changed, 15 insertions(+)
 create mode 100644 msg/actuator_controls_1.msg
 create mode 100644 msg/actuator_controls_2.msg
 create mode 100644 msg/actuator_controls_3.msg

diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg
new file mode 100644
index 0000000000..414eb06ddb
--- /dev/null
+++ b/msg/actuator_controls_1.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg
new file mode 100644
index 0000000000..414eb06ddb
--- /dev/null
+++ b/msg/actuator_controls_2.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control
diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg
new file mode 100644
index 0000000000..414eb06ddb
--- /dev/null
+++ b/msg/actuator_controls_3.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control

From f9b4a96bf254f12d7ef97a09fafbbff64a9d6e54 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 13:36:10 +0100
Subject: [PATCH 365/416] add msg/actuator_controls_virtual_fw.msg

---
 msg/actuator_controls_virtual_fw.msg | 5 +++++
 1 file changed, 5 insertions(+)
 create mode 100644 msg/actuator_controls_virtual_fw.msg

diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg
new file mode 100644
index 0000000000..414eb06ddb
--- /dev/null
+++ b/msg/actuator_controls_virtual_fw.msg
@@ -0,0 +1,5 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[8] control

From 265139f836b1fefdacb1e9498f0d25ecf2ecb47a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 13:36:36 +0100
Subject: [PATCH 366/416] add msg/fw_virtual_rates_setpoint.msg

---
 msg/fw_virtual_rates_setpoint.msg | 6 ++++++
 1 file changed, 6 insertions(+)
 create mode 100644 msg/fw_virtual_rates_setpoint.msg

diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg
new file mode 100644
index 0000000000..834113c798
--- /dev/null
+++ b/msg/fw_virtual_rates_setpoint.msg
@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1

From ee6395c5029737baeb328bd10ba82c8f05bf0648 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 13:38:47 +0100
Subject: [PATCH 367/416] enable mc att multiplatform in makefile

---
 makefiles/config_px4fmu-v2_multiplatform.mk | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index bfa26f83cf..76edade4b8 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -86,7 +86,8 @@ MODULES		+= modules/position_estimator_inav
 #MODULES		+= modules/fw_pos_control_l1
 #MODULES		+= modules/fw_att_control
 # MODULES		+= modules/mc_att_control
-# MODULES		+= modules/mc_att_control_multiplatform
+MODULES		+= modules/mc_att_control_multiplatform
+MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
 MODULES		+= modules/mc_pos_control
 MODULES 	+= modules/vtol_att_control

From 14aa5f9441759672a3c0d6ee9b6ff5256ae0a77e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 13:39:34 +0100
Subject: [PATCH 368/416] add topic header includes

---
 src/drivers/hil/hil.cpp                       | 22 ++++++++++---------
 src/drivers/mkblctrl/mkblctrl.cpp             |  1 +
 src/drivers/px4fmu/fmu.cpp                    |  6 ++++-
 src/drivers/px4io/px4io.cpp                   |  8 +++++--
 src/modules/bottle_drop/bottle_drop.cpp       |  4 ++++
 src/modules/commander/commander.cpp           |  4 ++++
 src/modules/controllib/uorb/blocks.hpp        |  4 ++++
 .../position_estimator_inav_main.c            |  4 ++++
 src/modules/sdlog2/sdlog2.c                   |  6 ++++-
 .../vtol_att_control_main.cpp                 |  4 ++++
 src/systemcmds/esc_calib/esc_calib.c          |  4 ++++
 11 files changed, 53 insertions(+), 14 deletions(-)

diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 9b5c8133b4..6ffa8252e0 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,8 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
@@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[])
 int
 HIL::set_mode(Mode mode)
 {
-	/* 
+	/*
 	 * Configure for PWM output.
 	 *
 	 * Note that regardless of the configured mode, the task is always
@@ -269,19 +271,19 @@ HIL::set_mode(Mode mode)
 		/* multi-port as 4 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		break;
-            
+
     	case MODE_8PWM:
             debug("MODE_8PWM");
             /* multi-port as 8 PWM outs */
             _update_rate = 50;	/* default output rate */
             break;
-            
+
         case MODE_12PWM:
             debug("MODE_12PWM");
             /* multi-port as 12 PWM outs */
             _update_rate = 50;	/* default output rate */
             break;
-            
+
         case MODE_16PWM:
             debug("MODE_16PWM");
             /* multi-port as 16 PWM outs */
@@ -513,12 +515,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		g_hil->set_pwm_rate(arg);
 		break;
 
 	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
-		// HIL always outputs at the alternate (usually faster) rate 
+		// HIL always outputs at the alternate (usually faster) rate
 		break;
 
 	case PWM_SERVO_SET(2):
@@ -659,7 +661,7 @@ int
 hil_new_mode(PortMode new_mode)
 {
 	// uint32_t gpio_bits;
-	
+
 
 //	/* reset to all-inputs */
 //	g_hil->ioctl(0, GPIO_RESET, 0);
@@ -691,17 +693,17 @@ hil_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode */
 		servo_mode = HIL::MODE_2PWM;
 		break;
-            
+
         case PORT2_8PWM:
             /* select 8-pin PWM mode */
             servo_mode = HIL::MODE_8PWM;
             break;
-            
+
         case PORT2_12PWM:
             /* select 12-pin PWM mode */
             servo_mode = HIL::MODE_12PWM;
             break;
-            
+
         case PORT2_16PWM:
             /* select 16-pin PWM mode */
             servo_mode = HIL::MODE_16PWM;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1055487cb9..a4505fe6fb 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,6 +74,7 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 112c015136..2552e7e6a6 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -70,6 +70,10 @@
 #include <drivers/drv_rc_input.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
@@ -1144,7 +1148,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		 * and PWM under control of the flight config
 		 * parameters. Note that this does not allow for
 		 * changing a set of pins to be used for serial on
-		 * FMUv1 
+		 * FMUv1
 		 */
 		switch (arg) {
 		case 0:
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 1c9a5adc9a..0451bbd1bd 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -75,6 +75,10 @@
 #include <systemlib/circuit_breaker.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
@@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state()
 
 		if (armed.ready_to_arm) {
 			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-			
+
 		} else {
 			clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 		}
@@ -2590,7 +2594,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_RC_CONFIG: {
 		/* enable setting of RC configuration without relying
-		   on param_get() 
+		   on param_get()
 		*/
 		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
 		if (config->channel >= RC_INPUT_MAX_CHANNELS) {
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index d8116ea11d..b267209fe3 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -57,6 +57,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 247a2c5b83..a1c4e205d4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -74,6 +74,10 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index a8a70507e9..491d4681de 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -47,6 +47,10 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/parameter_update.h>
 
 #include <stdio.h>
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 68ebd0f4f5..6de283396c 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -53,6 +53,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index a3407e42c4..ac08b0d237 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -70,6 +70,10 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -1117,7 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
 	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
-	
+
 	/* we need to rate-limit wind, as we do not need the full update rate */
 	orb_set_interval(subs.wind_sub, 90);
 	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 8e68730b8b..0a333eade6 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -58,7 +58,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vtol_vehicle_status.h>
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 20967b541d..32682f8908 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -64,6 +64,10 @@
 #include "drivers/drv_pwm_output.h"
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);

From 35efcaa92c575c60ad31f5a7ad9f8e5756cdf2bc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:01:03 +0100
Subject: [PATCH 369/416] update topic header includes for multiplatform
 headers

---
 src/platforms/px4_includes.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 90ec789960..ab27e6b4d1 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -76,6 +76,10 @@
 #include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_actuator_controls.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_0.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_1.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_2.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h>

From 1f0daea66586c32720083e5b2d703ce5fb9477f4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:01:37 +0100
Subject: [PATCH 370/416] remove uorb hacks

---
 src/modules/uORB/uORB.h | 19 -------------------
 1 file changed, 19 deletions(-)

diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index f19fbba7c5..672f8d8d1b 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -290,25 +290,6 @@ __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location
 #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
-ORB_DECLARE(actuator_controls_0);
-typedef struct actuator_controls_s actuator_controls_0_s;
-ORB_DECLARE(actuator_controls_1);
-typedef struct actuator_controls_s actuator_controls_1_s;
-ORB_DECLARE(actuator_controls_2);
-typedef struct actuator_controls_s actuator_controls_2_s;
-ORB_DECLARE(actuator_controls_3);
-typedef struct actuator_controls_s actuator_controls_3_s;
-ORB_DECLARE(actuator_controls_virtual_mc);
-typedef struct actuator_controls_s actuator_controls_virtual_mc_s;
-ORB_DECLARE(actuator_controls_virtual_fw);
-typedef struct actuator_controls_s actuator_controls_virtual_fw_s;
-ORB_DECLARE(mc_virtual_rates_setpoint);
-typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s;
-ORB_DECLARE(fw_virtual_rates_setpoint);
-typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s;
-ORB_DECLARE(mc_virtual_attitude_setpoint);
-typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s;
-ORB_DECLARE(fw_virtual_attitude_setpoint);
 typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
 typedef uint8_t arming_state_t;
 typedef uint8_t main_state_t;

From 8da83cfc80d2209e41cb47dd1dd2831d18a34012 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:02:06 +0100
Subject: [PATCH 371/416] objects common: use separate files

---
 src/modules/uORB/objects_common.cpp | 24 ++++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 78fdf4de75..204a114e12 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -121,8 +121,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
 
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
-ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
-ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
+#include "topics/mc_virtual_rates_setpoint.h"
+ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s);
+#include "topics/fw_virtual_rates_setpoint.h"
+ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s);
 
 #include "topics/rc_channels.h"
 ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -192,13 +194,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s);
 
 /* actuator controls, as requested by controller */
 #include "topics/actuator_controls.h"
-ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+#include "topics/actuator_controls_0.h"
+ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s);
+#include "topics/actuator_controls_1.h"
+ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s);
+#include "topics/actuator_controls_2.h"
+ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s);
+#include "topics/actuator_controls_3.h"
+ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s);
 //Virtual control groups, used for VTOL operation
-ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
+#include "topics/actuator_controls_virtual_mc.h"
+ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s);
+#include "topics/actuator_controls_virtual_fw.h"
+ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
 
 #include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);

From 7634147c0f9bb1ee27464d678394f22339b5ce00 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:02:43 +0100
Subject: [PATCH 372/416] starting to port mc att ctrl multiplatform to the
 latest api

---
 .../mc_att_control.cpp                        | 99 ++++++++++---------
 .../mc_att_control.h                          | 11 +--
 .../mc_att_control_base.cpp                   | 78 +++++++--------
 .../mc_att_control_base.h                     | 24 ++---
 .../mc_att_control_main.cpp                   |  4 +-
 .../mc_att_control_start_nuttx.cpp            | 99 +++++++++++++++++++
 6 files changed, 208 insertions(+), 107 deletions(-)
 create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
index 822a504b52..1e40c2b056 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
@@ -104,15 +104,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	/*
 	 * do subscriptions
 	 */
-	_v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
-	_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
-	_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
-	_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
-	_parameter_update = PX4_SUBSCRIBE(_n, parameter_update,
-			MulticopterAttitudeControl::handle_parameter_update, this, 1000);
-	_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
-	_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
-	_v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0);
+	_v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
+	_v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0);
+	_v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
+	_parameter_update = _n.subscribe<px4_parameter_update>(
+			&MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+	_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
+	_armed = _n.subscribe<px4_actuator_armed>(0);
+	_v_status = _n.subscribe<px4_vehicle_status>(0);
 
 }
 
@@ -180,12 +180,12 @@ MulticopterAttitudeControl::parameters_update()
 	return OK;
 }
 
-void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg)
+void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg)
 {
 	parameters_update();
 }
 
-void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
+void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
 
 	perf_begin(_loop_perf);
 
@@ -202,95 +202,98 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		dt = 0.02f;
 	}
 
-	if (_v_control_mode->get().flag_control_attitude_enabled) {
+	if (_v_control_mode->data().flag_control_attitude_enabled) {
 		control_attitude(dt);
 
 		/* publish the attitude setpoint if needed */
-		if (_publish_att_sp && _v_status->get().is_rotary_wing) {
-			_v_att_sp_mod.timestamp = px4::get_time_micros();
+		if (_publish_att_sp && _v_status->data().is_rotary_wing) {
+			_v_att_sp_mod.data().timestamp = px4::get_time_micros();
 
 			if (_att_sp_pub != nullptr) {
 				_att_sp_pub->publish(_v_att_sp_mod);
 
 			} else {
-				_att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint);
+				_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
 			}
 		}
 
 		/* publish attitude rates setpoint */
-		_v_rates_sp_mod.roll = _rates_sp(0);
-		_v_rates_sp_mod.pitch = _rates_sp(1);
-		_v_rates_sp_mod.yaw = _rates_sp(2);
-		_v_rates_sp_mod.thrust = _thrust_sp;
-		_v_rates_sp_mod.timestamp = px4::get_time_micros();
+		_v_rates_sp_mod.data().roll = _rates_sp(0);
+		_v_rates_sp_mod.data().pitch = _rates_sp(1);
+		_v_rates_sp_mod.data().yaw = _rates_sp(2);
+		_v_rates_sp_mod.data().thrust = _thrust_sp;
+		_v_rates_sp_mod.data().timestamp = px4::get_time_micros();
 
 		if (_v_rates_sp_pub != nullptr) {
 			_v_rates_sp_pub->publish(_v_rates_sp_mod);
 		} else {
-			if (_v_status->get().is_vtol) {
-				_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
+			if (_v_status->data().is_vtol) {
+				//XXX add a second publisher?
+				// _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
 			} else {
-				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+				_v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
 			}
 		}
 
 	} else {
 		/* attitude controller disabled, poll rates setpoint topic */
-		if (_v_control_mode->get().flag_control_manual_enabled) {
+		if (_v_control_mode->data().flag_control_manual_enabled) {
 			/* manual rates control - ACRO mode */
-			_rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x,
-						    _manual_control_sp->get().r).emult(_params.acro_rate_max);
-			_thrust_sp = _manual_control_sp->get().z;
+			_rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x,
+						    _manual_control_sp->data().r).emult(_params.acro_rate_max);
+			_thrust_sp = _manual_control_sp->data().z;
 
 			/* reset yaw setpoint after ACRO */
 			_reset_yaw_sp = true;
 
 			/* publish attitude rates setpoint */
-			_v_rates_sp_mod.roll = _rates_sp(0);
-			_v_rates_sp_mod.pitch = _rates_sp(1);
-			_v_rates_sp_mod.yaw = _rates_sp(2);
-			_v_rates_sp_mod.thrust = _thrust_sp;
-			_v_rates_sp_mod.timestamp = px4::get_time_micros();
+			_v_rates_sp_mod.data().roll = _rates_sp(0);
+			_v_rates_sp_mod.data().pitch = _rates_sp(1);
+			_v_rates_sp_mod.data().yaw = _rates_sp(2);
+			_v_rates_sp_mod.data().thrust = _thrust_sp;
+			_v_rates_sp_mod.data().timestamp = px4::get_time_micros();
 
 			if (_v_rates_sp_pub != nullptr) {
 				_v_rates_sp_pub->publish(_v_rates_sp_mod);
 
 			} else {
-				if (_v_status->get().is_vtol) {
-					_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
+				if (_v_status->data().is_vtol) {
+					//XXX add a second publisher?
+					// _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
 				} else {
-					_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+					_v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
 				}
 			}
 
 		} else {
 			/* attitude controller disabled, poll rates setpoint topic */
-			_rates_sp(0) = _v_rates_sp->get().roll;
-			_rates_sp(1) = _v_rates_sp->get().pitch;
-			_rates_sp(2) = _v_rates_sp->get().yaw;
-			_thrust_sp = _v_rates_sp->get().thrust;
+			_rates_sp(0) = _v_rates_sp->data().roll;
+			_rates_sp(1) = _v_rates_sp->data().pitch;
+			_rates_sp(2) = _v_rates_sp->data().yaw;
+			_thrust_sp = _v_rates_sp->data().thrust;
 		}
 	}
 
-	if (_v_control_mode->get().flag_control_rates_enabled) {
+	if (_v_control_mode->data().flag_control_rates_enabled) {
 		control_attitude_rates(dt);
 
 		/* publish actuator controls */
-		_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-		_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-		_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-		_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
-		_actuators.timestamp = px4::get_time_micros();
+		_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.data().timestamp = px4::get_time_micros();
 
 		if (!_actuators_0_circuit_breaker_enabled) {
 			if (_actuators_0_pub != nullptr) {
 				_actuators_0_pub->publish(_actuators);
 
 			} else {
-				if (_v_status->get().is_vtol) {
-					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc);
+				if (_v_status->data().is_vtol) {
+					//XXX add a second publisher?
+					// _actuators_0_pub = _n.advertise<px4_actuator_controls_virtual_mc>();
 				} else {
-					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
+					_actuators_0_pub = _n.advertise<px4_actuator_controls_0>();
 				}
 			}
 		}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h
index bff5289fdb..95d608684a 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h
@@ -59,7 +59,6 @@
 #include <math.h>
 #include <poll.h>
 #include <systemlib/perf_counter.h>
-// #include <systemlib/systemlib.h>
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 
@@ -80,8 +79,8 @@ public:
 	~MulticopterAttitudeControl();
 
 	/* Callbacks for topics */
-	void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
-	void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg);
+	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+	void handle_parameter_update(const px4_parameter_update &msg);
 
 	void spin() { _n.spin(); }
 
@@ -89,9 +88,9 @@ private:
 	bool		_task_should_exit;			/**< if true, sensor task should exit */
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
-	px4::Publisher *	_att_sp_pub;			/**< attitude setpoint publication */
-	px4::Publisher *	_v_rates_sp_pub;		/**< rate setpoint publication */
-	px4::Publisher *	_actuators_0_pub;		/**< attitude actuator controls publication */
+	px4::Publisher<px4_vehicle_attitude_setpoint> *	_att_sp_pub;			/**< attitude setpoint publication */
+	px4::Publisher<px4_vehicle_rates_setpoint> *	_v_rates_sp_pub;		/**< rate setpoint publication */
+	px4::Publisher<px4_actuator_controls_0> *	_actuators_0_pub;		/**< attitude actuator controls publication */
 
 	px4::NodeHandle _n;
 
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index aff59778ee..0de832df9e 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -92,28 +92,28 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	_publish_att_sp = false;
 
 
-	if (_v_control_mode->get().flag_control_manual_enabled) {
+	if (_v_control_mode->data().flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
 
-		if (_v_control_mode->get().flag_control_velocity_enabled
-		    || _v_control_mode->get().flag_control_climb_rate_enabled) {
+		if (_v_control_mode->data().flag_control_velocity_enabled
+		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod));
+			memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
 		}
 
-		if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
+		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp_mod.thrust = _manual_control_sp->get().z;
+			_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
 			_publish_att_sp = true;
 		}
 
-		if (!_armed->get().armed) {
+		if (!_armed->data().armed) {
 			/* reset yaw setpoint when disarmed */
 			_reset_yaw_sp = true;
 		}
 
 		/* move yaw setpoint in all modes */
-		if (_v_att_sp_mod.thrust < 0.1f) {
+		if (_v_att_sp_mod.data().thrust < 0.1f) {
 			// TODO
 			//if (_status.condition_landed) {
 			/* reset yaw setpoint if on ground */
@@ -121,71 +121,71 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max;
-			_v_att_sp_mod.yaw_body = _wrap_pi(
-						     _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt);
+			yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
+			_v_att_sp_mod.data().yaw_body = _wrap_pi(
+						     _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
 			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw);
+			float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
 
 			if (yaw_offs < -yaw_offs_max) {
-				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
+				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
+				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
 			}
 
-			_v_att_sp_mod.R_valid = false;
+			_v_att_sp_mod.data().R_valid = false;
 			// _publish_att_sp = true;
 		}
 
 		/* reset yaw setpint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
-			_v_att_sp_mod.yaw_body = _v_att->get().yaw;
-			_v_att_sp_mod.R_valid = false;
+			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
+			_v_att_sp_mod.data().R_valid = false;
 			// _publish_att_sp = true;
 		}
 
-		if (!_v_control_mode->get().flag_control_velocity_enabled) {
+		if (!_v_control_mode->data().flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
-			_v_att_sp_mod.pitch_body = -_manual_control_sp->get().x
+			_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
+			_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
 					       * _params.man_pitch_max;
-			_v_att_sp_mod.R_valid = false;
+			_v_att_sp_mod.data().R_valid = false;
 			// _publish_att_sp = true;
 		}
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod));
+		memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
 	}
 
-	_thrust_sp = _v_att_sp_mod.thrust;
+	_thrust_sp = _v_att_sp_mod.data().thrust;
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
 
-	if (_v_att_sp_mod.R_valid) {
+	if (_v_att_sp_mod.data().R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp_mod.R_body[0]);
+		R_sp.set(&_v_att_sp_mod.data().R_body[0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body,
-				_v_att_sp_mod.yaw_body);
+		R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
+				_v_att_sp_mod.data().yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0],
-		       sizeof(_v_att_sp_mod.R_body));
-		_v_att_sp_mod.R_valid = true;
+		memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
+		       sizeof(_v_att_sp_mod.data().R_body));
+		_v_att_sp_mod.data().R_valid = true;
 	}
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
-	R.set(_v_att->get().R);
+	R.set(_v_att->data().R);
 
 	/* all input data is ready, run controller itself */
 
@@ -266,15 +266,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed->get().armed || !_v_status->get().is_rotary_wing) {
+	if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
 		_rates_int.zero();
 	}
 
 	/* current body angular rates */
 	math::Vector < 3 > rates;
-	rates(0) = _v_att->get().rollspeed;
-	rates(1) = _v_att->get().pitchspeed;
-	rates(2) = _v_att->get().yawspeed;
+	rates(0) = _v_att->data().rollspeed;
+	rates(1) = _v_att->data().pitchspeed;
+	rates(2) = _v_att->data().yawspeed;
 
 	/* angular rates error */
 	math::Vector < 3 > rates_err = _rates_sp - rates;
@@ -302,8 +302,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 
 void MulticopterAttitudeControlBase::set_actuator_controls()
 {
-	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+	_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+	_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+	_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+	_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 }
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
index cf4c726a78..124147079a 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
@@ -62,6 +62,8 @@
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
+using namespace px4;
+
 class MulticopterAttitudeControlBase
 {
 public:
@@ -86,20 +88,20 @@ public:
 	void set_actuator_controls();
 
 protected:
-	px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att;			    /**< vehicle attitude */
-	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	    /**< vehicle attitude setpoint */
-	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	    /**< vehicle rates setpoint */
-	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
-	px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update;	    /**< parameter update */
-	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;   /**< manual control setpoint */
-	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
-	px4::PX4_SUBSCRIBER(vehicle_status) *_v_status;			    /**< vehicle status */
+	px4::Subscriber<px4_vehicle_attitude> *_v_att;			    /**< vehicle attitude */
+	px4::Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
+	px4::Subscriber<px4_vehicle_rates_setpoint> *_v_rates_sp;	    /**< vehicle rates setpoint */
+	px4::Subscriber<px4_vehicle_control_mode> *_v_control_mode;	    /**< vehicle control mode */
+	px4::Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
+	px4::Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
+	px4::Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
+	px4::Subscriber<px4_vehicle_status> *_v_status;			    /**< vehicle status */
 
-	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
+	px4_vehicle_attitude_setpoint	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
 								  that gets published eventually */
-	PX4_TOPIC_T(vehicle_rates_setpoint)	_v_rates_sp_mod;	/**< vehicle rates setpoint
+	px4_vehicle_rates_setpoint	_v_rates_sp_mod;	/**< vehicle rates setpoint
 								  that gets published eventually*/
-	PX4_TOPIC_T(actuator_controls)		_actuators;	/**< actuator controls */
+	px4_actuator_controls_0		_actuators;	/**< actuator controls */
 
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */
 	math::Vector<3> _rates_sp; /**< angular rates setpoint */
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
index 67ebe64cdc..b356b5dc0b 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -52,11 +52,9 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
-#include <string.h>
-#include <cstdlib>
 #include "mc_att_control.h"
 
-static bool thread_running = false;     /**< Deamon status flag */
+bool thread_running = false;     /**< Deamon status flag */
 static int daemon_task;             /**< Handle of deamon task / thread */
 namespace px4
 {
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
new file mode 100644
index 0000000000..e982ae3545
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_m_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
+int mc_att_control_m_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_att_control_m {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_att_control_m",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

From cd35ab26610650d93120d3a9eabd196a733c2e44 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:32:40 +0100
Subject: [PATCH 373/416] add another set of uorb headers

---
 src/drivers/roboclaw/RoboClaw.cpp     | 15 ++++++++-------
 src/examples/fixedwing_control/main.c | 20 ++++++++++++--------
 src/examples/hwtest/hwtest.c          |  4 ++++
 3 files changed, 24 insertions(+), 15 deletions(-)

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index eb9453b503..5b945e452d 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -54,6 +54,7 @@
 #include <mavlink/mavlink_log.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	uint8_t rbuf[50];
 	usleep(5000);
 	int nread = read(_uart, rbuf, 50);
-	if (nread < 6) { 
+	if (nread < 6) {
 		printf("failed to read\n");
 		return -1;
 	}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
 	uint8_t checksum = rbuf[5];
-	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
 		checksum_mask;
 	// check if checksum is valid
 	if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
 	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
-		_motor1Position = float(int64_t(count) + 
+		_motor1Position = float(int64_t(count) +
 			_motor1Overflow*overflowAmount)/_pulsesPerRev;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
-		_motor2Position = float(int64_t(count) + 
+		_motor2Position = float(int64_t(count) +
 			_motor2Overflow*overflowAmount)/_pulsesPerRev;
 	}
 	return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	return -1;
 }
 
-int RoboClaw::resetEncoders() 
+int RoboClaw::resetEncoders()
 {
 	uint16_t sum = 0;
 	return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 	return sum;
 }
 
-int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 		size_t n_data, uint16_t & prev_sum)
 {
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 	return write(_uart, buf, n_data + 3);
 }
 
-int roboclawTest(const char *deviceName, uint8_t address, 
+int roboclawTest(const char *deviceName, uint8_t address,
 		uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index fcbb54c8ea..d8b777b91b 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -61,6 +61,10 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
@@ -108,7 +112,7 @@ static void usage(const char *reason);
  * @param att The current attitude. The controller should make the attitude match the setpoint
  * @param rates_sp The angular rate setpoint. This is the output of the controller.
  */
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
 	struct actuator_controls_s *actuators);
 
 /**
@@ -133,18 +137,18 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 static struct params p;
 static struct param_handles ph;
 
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
 	struct actuator_controls_s *actuators)
 {
 
-	/* 
+	/*
 	 * The PX4 architecture provides a mixer outside of the controller.
 	 * The mixer is fed with a default vector of actuator controls, representing
 	 * moments applied to the vehicle frame. This vector
 	 * is structured as:
 	 *
 	 * Control Group 0 (attitude):
-	 * 
+	 *
 	 *    0  -  roll   (-1..+1)
 	 *    1  -  pitch  (-1..+1)
 	 *    2  -  yaw    (-1..+1)
@@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	 *
 	 * Wikipedia description:
 	 * http://en.wikipedia.org/wiki/Publish–subscribe_pattern
-	 * 
+	 *
 	 */
 
 
@@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 	/*
 	 * Declare and safely initialize all structs to zero.
-	 * 
+	 *
 	 * These structs contain the system state and things
 	 * like attitude, position, the current waypoint, etc.
 	 */
@@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/*
 			 * Poll error, this will not really happen in practice,
-			 * but its good design practice to make output an error message. 
+			 * but its good design practice to make output an error message.
 			 */
 			warnx("poll error");
 
@@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				}
 
 				if (manual_sp_updated)
-					/* get the RC (or otherwise user based) input */ 
+					/* get the RC (or otherwise user based) input */
 					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
 
 				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
index 95ff346bbb..8fd8870da7 100644
--- a/src/examples/hwtest/hwtest.c
+++ b/src/examples/hwtest/hwtest.c
@@ -47,6 +47,10 @@
 #include <systemlib/err.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/uORB.h>
 
 __EXPORT int ex_hwtest_main(int argc, char *argv[]);

From dafe6654ebd2730c0ac5c3c60bb5fe8946ab67a0 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:35:25 +0100
Subject: [PATCH 374/416] add missing functional header

---
 src/platforms/px4_nodehandle.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 80be9ec527..83a3e422dd 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -53,6 +53,7 @@
 /* includes when building for NuttX */
 #include <poll.h>
 #endif
+#include <functional>
 
 namespace px4
 {

From 23229317ad5e08f758a4d48148d36fabe2a2cfbd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:35:42 +0100
Subject: [PATCH 375/416] remove unnecessary include

---
 src/platforms/px4_includes.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index ab27e6b4d1..1bd4509caf 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -70,7 +70,6 @@
  */
 #include <nuttx/config.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/rc_channels.h>
 #ifdef __cplusplus
 #include <platforms/nuttx/px4_messages/px4_rc_channels.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h>

From 9b4a21049550f12f11ec0e6dd2d2a55e1b2ef1b3 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 15:55:32 +0100
Subject: [PATCH 376/416] missing headers for fmu2 target

---
 src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++
 src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++
 2 files changed, 8 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index e0b61e2e27..88918333d2 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -59,7 +59,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
 #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/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 228f142d97..562033db1d 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -63,7 +63,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
 #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/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>

From f7dc81ded1bdd7eb7d88532e5513b168c6d4f118 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 16:13:39 +0100
Subject: [PATCH 377/416] missing headers for fmu1 target

---
 src/drivers/ardrone_interface/ardrone_interface.c | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 7f1b21a95c..7d4b7d880b 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -55,6 +55,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
@@ -98,7 +102,7 @@ usage(const char *reason)
  * The deamon 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().
  */
@@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-			
+
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */

From 5cb208c32f5baf52fffb97918f5b06b2498aec87 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 18:22:13 +0100
Subject: [PATCH 378/416] reenable mcatt ctl and other nodes for ros

---
 CMakeLists.txt | 84 +++++++++++++++++++++++++-------------------------
 1 file changed, 42 insertions(+), 42 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 25822d7190..f76dbbf418 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -169,52 +169,52 @@ target_link_libraries(subscriber
   px4
 )
 
-# ## MC Attitude Control
-# add_executable(mc_att_control
-	# src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
-	# src/modules/mc_att_control_multiplatform/mc_att_control.cpp
-	# src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
-# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
-# target_link_libraries(mc_att_control
-  # ${catkin_LIBRARIES}
-  # px4
-# )
+## MC Attitude Control
+add_executable(mc_att_control
+	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
+add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_att_control
+  ${catkin_LIBRARIES}
+  px4
+)
 
-# ## Attitude Estimator dummy
-# add_executable(attitude_estimator
-	# src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
-# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
-# target_link_libraries(attitude_estimator
-  # ${catkin_LIBRARIES}
-  # px4
-# )
+## Attitude Estimator dummy
+add_executable(attitude_estimator
+	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(attitude_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
 
-# ## Manual input
-# add_executable(manual_input
-	# src/platforms/ros/nodes/manual_input/manual_input.cpp)
-# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-# target_link_libraries(manual_input
-  # ${catkin_LIBRARIES}
-  # px4
-# )
+## Manual input
+add_executable(manual_input
+	src/platforms/ros/nodes/manual_input/manual_input.cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(manual_input
+  ${catkin_LIBRARIES}
+  px4
+)
 
-# ## Multicopter Mixer dummy
-# add_executable(mc_mixer
-  # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
-# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
-# target_link_libraries(mc_mixer
-  # ${catkin_LIBRARIES}
-  # px4
-# )
+## Multicopter Mixer dummy
+add_executable(mc_mixer
+  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
+add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_mixer
+  ${catkin_LIBRARIES}
+  px4
+)
 
-# ## Commander
-# add_executable(commander
-	# src/platforms/ros/nodes/commander/commander.cpp)
-# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-# target_link_libraries(commander
-  # ${catkin_LIBRARIES}
-  # px4
-# )
+## Commander
+add_executable(commander
+	src/platforms/ros/nodes/commander/commander.cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(commander
+  ${catkin_LIBRARIES}
+  px4
+)
 
 #############
 ## Install ##

From 092a3c5129e4d26463b298d110dbfa8683fb9c26 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 18:23:46 +0100
Subject: [PATCH 379/416] add start wrapper for mc att ctl multip

---
 .../mc_att_control_main.cpp                   | 66 +------------------
 .../mc_att_control_multiplatform/module.mk    |  1 +
 2 files changed, 2 insertions(+), 65 deletions(-)

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
index b356b5dc0b..5a79a8c6b2 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -55,72 +55,8 @@
 #include "mc_att_control.h"
 
 bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-namespace px4
-{
-bool task_should_exit = false;
-}
 
-using namespace px4;
-
-PX4_MAIN_FUNCTION(mc_att_control_m);
-
-#if !defined(__PX4_ROS)
-/**
- * Multicopter attitude control app start / stop handling function
- *
- * @ingroup apps
- */
-
-extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
-int mc_att_control_m_main(int argc, char *argv[])
-{
-	if (argc < 1) {
-		errx(1, "usage: mc_att_control_m {start|stop|status}");
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		task_should_exit = false;
-
-		daemon_task = task_spawn_cmd("mc_att_control_m",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       3000,
-				       mc_att_control_m_task_main,
-					(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		task_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running");
-
-		} else {
-			warnx("not started");
-		}
-
-		exit(0);
-	}
-
-	warnx("unrecognized command");
-	return 1;
-}
-#endif
-
-PX4_MAIN_FUNCTION(mc_att_control_m)
+int main(int argc, char **argv)
 {
 	px4::init(argc, argv, "mc_att_control_m");
 
diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk
index c61ba18b42..959f6492b0 100644
--- a/src/modules/mc_att_control_multiplatform/module.mk
+++ b/src/modules/mc_att_control_multiplatform/module.mk
@@ -38,6 +38,7 @@
 MODULE_COMMAND	= mc_att_control_m
 
 SRCS		= mc_att_control_main.cpp \
+		  mc_att_control_start_nuttx.cpp \
 		  mc_att_control.cpp \
 		  mc_att_control_base.cpp \
 		  mc_att_control_params.c

From 8dbb1fc8d443b1f8e2e454d4b82051fa63a48398 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 25 Jan 2015 18:24:10 +0100
Subject: [PATCH 380/416] remove unneeded include

---
 src/platforms/px4_defines.h | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 325832f0f1..c8e2cf290b 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -141,9 +141,6 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
  * Building for NuttX
  */
 #include <platforms/px4_includes.h>
-#ifdef __cplusplus
-#include <functional>
-#endif
 /* Main entry point */
 #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[])
 

From abeae7b6f6d70042c6b0bed493ade52727648c2d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 26 Jan 2015 14:50:23 +0100
Subject: [PATCH 381/416] extend subscriber/publisher example

---
 src/examples/publisher/publisher_example.cpp  | 25 ++++++++++++++----
 src/examples/publisher/publisher_example.h    |  2 ++
 .../subscriber/subscriber_example.cpp         | 26 ++++++++++++++++---
 src/examples/subscriber/subscriber_example.h  |  3 ++-
 4 files changed, 47 insertions(+), 9 deletions(-)

diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index 9952a16e4b..e7ab9eb5af 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -45,7 +45,9 @@ using namespace px4;
 
 PublisherExample::PublisherExample() :
 	_n(),
-	_rc_channels_pub(_n.advertise<px4_rc_channels>())
+	_rc_channels_pub(_n.advertise<px4_rc_channels>()),
+	_v_att_pub(_n.advertise<px4_vehicle_attitude>()),
+	_parameter_update_pub(_n.advertise<px4_parameter_update>())
 {
 }
 
@@ -57,10 +59,23 @@ int PublisherExample::main()
 		loop_rate.sleep();
 		_n.spinOnce();
 
-		px4_rc_channels msg;
-		msg.data().timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.data().timestamp_last_valid);
-		_rc_channels_pub->publish(msg);
+		/* Publish example message */
+		px4_rc_channels rc_channels_msg;
+		rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid);
+		_rc_channels_pub->publish(rc_channels_msg);
+
+		/* Publish example message */
+		px4_vehicle_attitude v_att_msg;
+		v_att_msg.data().timestamp = px4::get_time_micros();
+		PX4_INFO("att: %llu", v_att_msg.data().timestamp);
+		_v_att_pub->publish(v_att_msg);
+
+		/* Publish example message */
+		px4_parameter_update parameter_update_msg;
+		parameter_update_msg.data().timestamp = px4::get_time_micros();
+		PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp);
+		_parameter_update_pub->publish(parameter_update_msg);
 
 	}
 
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 4ff59a226d..8165b0ffce 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -50,4 +50,6 @@ public:
 protected:
 	px4::NodeHandle _n;
 	px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
+	px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub;
+	px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub;
 };
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 781dde486c..e1292f7888 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -67,9 +67,15 @@ SubscriberExample::SubscriberExample() :
 	/* No callback */
 	_sub_rc_chan = _n.subscribe<px4_rc_channels>(500);
 
-	/* Class Method */
+	/* Class method */
 	_n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000);
 
+	/* Another class method */
+	_n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000);
+
+	/* Yet antoher class method */
+	_n.subscribe<px4_parameter_update>(&SubscriberExample::parameter_update_callback, this, 1000);
+
 	PX4_INFO("subscribed");
 }
 
@@ -78,8 +84,22 @@ SubscriberExample::SubscriberExample() :
  * Also the current value of the _sub_rc_chan subscription is printed
  */
 void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
-	PX4_INFO("Callback (method): [%llu]",
+	PX4_INFO("rc_channels_callback (method): [%llu]",
 			msg.data().timestamp_last_valid);
-	PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]",
+	PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]",
 			_sub_rc_chan->data().timestamp_last_valid);
 }
+
+void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) {
+	PX4_INFO("vehicle_attitude_callback (method): [%llu]",
+			msg.data().timestamp);
+}
+
+void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) {
+	PX4_INFO("parameter_update_callback (method): [%llu]",
+			msg.data().timestamp);
+	PX4_PARAM_GET(_p_sub_interv, &_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _interval);
+	PX4_PARAM_GET(_p_test_float, &_test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
+}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 8da3df4387..9b6d890e3b 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -59,6 +59,7 @@ protected:
 	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
 	void rc_channels_callback(const px4_rc_channels &msg);
-
+	void vehicle_attitude_callback(const px4_vehicle_attitude &msg);
+	void parameter_update_callback(const px4_parameter_update &msg);
 
 };

From de44c549eb8d195a0356afdd875b23a277fd8c7a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 26 Jan 2015 15:53:49 +0100
Subject: [PATCH 382/416] mc attctl multiplatform: remove memsets

---
 .../mc_att_control_multiplatform/mc_att_control_base.cpp   | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index 0de832df9e..fcfee28dc7 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -55,13 +55,12 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+	_v_att_sp_mod(),
+	_v_rates_sp_mod(),
+	_actuators(),
 	_publish_att_sp(false)
 
 {
-	memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod));
-	memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod));
-	memset(&_actuators, 0, sizeof(_actuators));
-
 	_params.att_p.zero();
 	_params.rate_p.zero();
 	_params.rate_i.zero();

From f23e603d02ba416ae250770cdaad6a859d6bae69 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 26 Jan 2015 15:54:19 +0100
Subject: [PATCH 383/416] mc attctl multiplatform: increase stack size

---
 .../mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
index e982ae3545..c2b8470757 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
@@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("mc_att_control_m",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       3000,
 				       main,
 					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
 

From 856b10cc1a5a36a7b7d2978477185155792366c2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 16:29:25 +0100
Subject: [PATCH 384/416] Revert "temporarily re-enable stack checking, disable
 some modules to make firmware fit"

This reverts commit 27b2701340648e2fde1992b175abfa591e0eee01.
---
 makefiles/config_px4fmu-v2_default.mk | 10 +++++-----
 nuttx-configs/px4fmu-v2/nsh/defconfig |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 5d6beffd7e..2ce7388446 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -37,11 +37,11 @@ MODULES		+= drivers/gps
 # MODULES		+= drivers/hott/hott_sensors
 # MODULES		+= drivers/blinkm
 MODULES		+= drivers/airspeed
-# MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
-# MODULES		+= drivers/frsky_telemetry
+MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
-# MODULES		+= drivers/mkblctrl
+MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/px4flow
 
 #
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-# MODULES		+= modules/uavcan
+MODULES		+= modules/uavcan
 MODULES 	+= modules/land_detector
 
 #
@@ -121,7 +121,7 @@ MODULES		+= platforms/nuttx
 #
 # OBC challenge
 #
-# MODULES		+= modules/bottle_drop
+MODULES		+= modules/bottle_drop
 
 #
 # Demo apps
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 9030a1f022..dedebdfa03 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y
 #
 # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
-CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_ARMV7M_STACKCHECK=n
 CONFIG_SERIAL_TERMIOS=y
 CONFIG_SDIO_DMA=y
 CONFIG_SDIO_DMAPRIO=0x00010000

From c67cb25f9a4cb4ec507029865dd70837417ef894 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 07:54:58 +0100
Subject: [PATCH 385/416] port more uorb headers to msg

---
 msg/position_setpoint.msg                     |  35 ++++++
 msg/position_setpoint_triplet.msg             |   8 ++
 msg/vehicle_global_velocity_setpoint.msg      |   4 +
 msg/vehicle_local_position.msg                |  36 ++++++
 msg/vehicle_local_position_setpoint.msg       |   7 ++
 .../uORB/topics/position_setpoint_triplet.h   | 116 ------------------
 .../topics/vehicle_global_velocity_setpoint.h |  63 ----------
 .../uORB/topics/vehicle_local_position.h      |  96 ---------------
 .../topics/vehicle_local_position_setpoint.h  |  67 ----------
 9 files changed, 90 insertions(+), 342 deletions(-)
 create mode 100644 msg/position_setpoint.msg
 create mode 100644 msg/position_setpoint_triplet.msg
 create mode 100644 msg/vehicle_global_velocity_setpoint.msg
 create mode 100644 msg/vehicle_local_position.msg
 create mode 100644 msg/vehicle_local_position_setpoint.msg
 delete mode 100644 src/modules/uORB/topics/position_setpoint_triplet.h
 delete mode 100644 src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
 delete mode 100644 src/modules/uORB/topics/vehicle_local_position.h
 delete mode 100644 src/modules/uORB/topics/vehicle_local_position_setpoint.h

diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg
new file mode 100644
index 0000000000..ae6756aa8b
--- /dev/null
+++ b/msg/position_setpoint.msg
@@ -0,0 +1,35 @@
+# this file is only used in the position_setpoint triple as a dependency
+
+uint8 SETPOINT_TYPE_POSITION=0	# position setpoint
+uint8 SETPOINT_TYPE_VELOCITY=1	# velocity setpoint
+uint8 SETPOINT_TYPE_LOITER=2	# loiter setpoint
+uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
+uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
+uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
+uint8 SETPOINT_TYPE_OFFBOARD=6 	# setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard
+
+bool valid			# true if setpoint is valid
+uint8 type			# setpoint type to adjust behavior of position controller
+float32 x			# local position setpoint in m in NED
+float32 y			# local position setpoint in m in NED
+float32 z			# local position setpoint in m in NED
+bool position_valid		# true if local position setpoint valid
+float32 vx			# local velocity setpoint in m/s in NED
+float32 vy			# local velocity setpoint in m/s in NED
+float32 vz			# local velocity setpoint in m/s in NED
+bool velocity_valid		# true if local velocity setpoint valid
+float64 lat			# latitude, in deg
+float64 lon			# longitude, in deg
+float32 alt			# altitude AMSL, in m
+float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
+bool yaw_valid			# true if yaw setpoint valid
+float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
+bool yawspeed_valid		# true if yawspeed setpoint valid
+float32 loiter_radius		# loiter radius (only for fixed wing), in m
+int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
+float32 pitch_min		# minimal pitch angle for fixed wing takeoff waypoints
+float32 a_x			# acceleration x setpoint
+float32 a_y			# acceleration y setpoint
+float32 a_z			# acceleration z setpoint
+bool acceleration_valid		# true if acceleration setpoint is valid/should be used
+bool acceleration_is_force	# interprete acceleration as force
diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg
new file mode 100644
index 0000000000..8717f65d0f
--- /dev/null
+++ b/msg/position_setpoint_triplet.msg
@@ -0,0 +1,8 @@
+# Global position setpoint triplet in WGS84 coordinates.
+# This are the three next waypoints (or just the next two or one).
+
+px4/position_setpoint previous
+px4/position_setpoint current
+px4/position_setpoint next
+
+uint8 nav_state	# report the navigation state
diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg
new file mode 100644
index 0000000000..ca7dcc826a
--- /dev/null
+++ b/msg/vehicle_global_velocity_setpoint.msg
@@ -0,0 +1,4 @@
+# Velocity setpoint in NED frame
+float32 vx		# in m/s NED
+float32 vy		# in m/s NED
+float32 vz		# in m/s NED
diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg
new file mode 100644
index 0000000000..4da027ae77
--- /dev/null
+++ b/msg/vehicle_local_position.msg
@@ -0,0 +1,36 @@
+# Fused local position in NED.
+
+uint64 timestamp		# Time of this estimate, in microseconds since system start
+bool xy_valid			# true if x and y are valid
+bool z_valid			# true if z is valid
+bool v_xy_valid		# true if vy and vy are valid
+bool v_z_valid			# true if vz is valid
+
+# Position in local NED frame
+float32 x				# X position in meters in NED earth-fixed frame
+float32 y				# X position in meters in NED earth-fixed frame
+float32 z				# Z position in meters in NED earth-fixed frame (negative altitude)
+
+# Velocity in NED frame
+float32 vx 				# Ground X Speed (Latitude), m/s in NED
+float32 vy				# Ground Y Speed (Longitude), m/s in NED
+float32 vz				# Ground Z Speed (Altitude), m/s	in NED
+
+# Heading
+float32 yaw
+
+# Reference position in GPS / WGS84 frame
+bool xy_global			# true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+bool z_global			# true if z is valid and has valid global reference (ref_alt)
+uint64 ref_timestamp	# Time when reference position was set
+float64 ref_lat		# Reference point latitude in degrees
+float64 ref_lon		# Reference point longitude in degrees
+float32 ref_alt			# Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
+
+# Distance to surface
+float32 dist_bottom		# Distance to bottom surface (ground)
+float32 dist_bottom_rate		# Distance to bottom surface (ground) change rate
+uint64 surface_bottom_timestamp		# Time when new bottom surface found
+bool dist_bottom_valid	# true if distance to bottom surface is valid
+float32 eph
+float32 epv
diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg
new file mode 100644
index 0000000000..a2ff8a4ae7
--- /dev/null
+++ b/msg/vehicle_local_position_setpoint.msg
@@ -0,0 +1,7 @@
+# Local position in NED frame
+
+uint64 timestamp	# timestamp of the setpoint
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+float32 yaw		# in radians NED -PI..+PI
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
deleted file mode 100644
index cb2262534d..0000000000
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mission_item_triplet.h
- * Definition of the global WGS84 position setpoint uORB topic.
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
-#define TOPIC_MISSION_ITEM_TRIPLET_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-enum SETPOINT_TYPE
-{
-	SETPOINT_TYPE_POSITION = 0,	/**< position setpoint */
-	SETPOINT_TYPE_VELOCITY,		/**< velocity setpoint */
-	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
-	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
-	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
-	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
-	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
-};
-
-struct position_setpoint_s
-{
-	bool valid;			/**< true if setpoint is valid */
-	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
-	float x;			/**< local position setpoint in m in NED */
-	float y;			/**< local position setpoint in m in NED */
-	float z;			/**< local position setpoint in m in NED */
-	bool position_valid;	/**< true if local position setpoint valid */
-	float vx;			/**< local velocity setpoint in m/s in NED */
-	float vy;			/**< local velocity setpoint in m/s in NED */
-	float vz;			/**< local velocity setpoint in m/s in NED */
-	bool velocity_valid;		/**< true if local velocity setpoint valid */
-	double lat;			/**< latitude, in deg */
-	double lon;			/**< longitude, in deg */
-	float alt;			/**< altitude AMSL, in m */
-	float yaw;			/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
-	bool yaw_valid;			/**< true if yaw setpoint valid */
-	float yawspeed;			/**< yawspeed (only for multirotors, in rad/s) */
-	bool yawspeed_valid;		/**< true if yawspeed setpoint valid */
-	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
-	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
-	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
-	float a_x;			//**< acceleration x setpoint */
-	float a_y;			//**< acceleration y setpoint */
-	float a_z;			//**< acceleration z setpoint */
-	bool acceleration_valid;	//*< true if acceleration setpoint is valid/should be used */
-	bool acceleration_is_force;	//*< interprete acceleration as force */
-};
-
-/**
- * Global position setpoint triplet in WGS84 coordinates.
- *
- * This are the three next waypoints (or just the next two or one).
- */
-struct position_setpoint_triplet_s
-{
-	struct position_setpoint_s previous;
-	struct position_setpoint_s current;
-	struct position_setpoint_s next;
-
-	unsigned nav_state;				/**< report the navigation state */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(position_setpoint_triplet);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
deleted file mode 100644
index 5dac877d08..0000000000
--- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author 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 vehicle_global_velocity_setpoint.h
- * Definition of the global velocity setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
-
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct vehicle_global_velocity_setpoint_s {
-	float vx;		/**< in m/s NED			  		*/
-	float vy;		/**< in m/s NED			  		*/
-	float vz;		/**< in m/s NED			  		*/
-}; /**< Velocity setpoint in NED frame */
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_velocity_setpoint);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
deleted file mode 100644
index 8b46c5a3f3..0000000000
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_local_position.h
- * Definition of the local fused NED position uORB topic.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
-#define TOPIC_VEHICLE_LOCAL_POSITION_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Fused local position in NED.
- */
-struct vehicle_local_position_s {
-	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
-	bool xy_valid;			/**< true if x and y are valid */
-	bool z_valid;			/**< true if z is valid */
-	bool v_xy_valid;		/**< true if vy and vy are valid */
-	bool v_z_valid;			/**< true if vz is valid */
-	/* Position in local NED frame */
-	float x;				/**< X position in meters in NED earth-fixed frame */
-	float y;				/**< X position in meters in NED earth-fixed frame */
-	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
-	/* Velocity in NED frame */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
-	/* Heading */
-	float yaw;
-	/* Reference position in GPS / WGS84 frame */
-	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
-	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
-	uint64_t ref_timestamp;	/**< Time when reference position was set */
-	double ref_lat;		/**< Reference point latitude in degrees */
-	double ref_lon;		/**< Reference point longitude in degrees */
-	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
-	/* Distance to surface */
-	float dist_bottom;		/**< Distance to bottom surface (ground) */
-	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
-	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
-	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
-	float eph;
-	float epv;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_local_position);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
deleted file mode 100644
index 6766bb58a9..0000000000
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 vehicle_local_position_setpoint.h
- * Definition of the local NED position setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
-
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct vehicle_local_position_setpoint_s {
-	uint64_t timestamp;	/**< timestamp of the setpoint */
-	float x;		/**< in meters NED			  		*/
-	float y;		/**< in meters NED			  		*/
-	float z;		/**< in meters NED			  		*/
-	float yaw;		/**< in radians NED -PI..+PI  		*/
-}; /**< Local position in NED frame */
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_local_position_setpoint);
-
-#endif

From 01835a51a8a3a0b0f7e7362cdc25475bd029a9a8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 07:55:21 +0100
Subject: [PATCH 386/416] uorb constants are now defined inside class

---
 msg/templates/msg.h.template      | 159 ------------------------------
 msg/templates/uorb/msg.h.template |  23 ++++-
 2 files changed, 21 insertions(+), 161 deletions(-)
 delete mode 100644 msg/templates/msg.h.template

diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
deleted file mode 100644
index cc128c1ea2..0000000000
--- a/msg/templates/msg.h.template
+++ /dev/null
@@ -1,159 +0,0 @@
-@###############################################
-@#
-@# PX4 ROS compatible message source code
-@# generation for C++
-@#
-@# EmPy template for generating <msg>.h files
-@# Based on the original template for ROS
-@#
-@###############################################
-@# Start of Template
-@#
-@# Context:
-@#  - file_name_in (String) Source file
-@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
-@#  - md5sum (String) MD5Sum of the .msg specification
-@###############################################
-/****************************************************************************
- *
- *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
- /* Auto-generated by genmsg_cpp from file @file_name_in */
-
-@{
-import genmsg.msgs
-import gencpp
-
-cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace
-cpp_class = '%s_'%spec.short_name
-cpp_full_name = '%s%s'%(cpp_namespace,cpp_class)
-cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name)
-cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
-}@
-
-#pragma once
-
-@##############################
-@# Generic Includes
-@##############################
-#include <stdint.h>
-#include "../uORB.h"
-
-@##############################
-@# Includes for dependencies
-@##############################
-@{
-for field in spec.parsed_fields():
-  if (not field.is_builtin):
-    if (not field.is_header):
-      (package, name) = genmsg.names.package_resource_name(field.base_type)
-      package = package or spec.package # convert '' to package
-      print('#include <uORB/topics/%s.h>'%(name))
-
-}@
-@# Constants
-@[for constant in spec.constants]@
-#define @(constant.name) @(int(constant.val))
-@[end for]
-
-/**
- * @@addtogroup topics
- * @@{
- */
-
-@##############################
-@# Main struct of message
-@##############################
-@{
-
-type_map = {'int8': 'int8_t',
-  'int16': 'int16_t',
-  'int32': 'int32_t',
-  'int64': 'int64_t',
-  'uint8': 'uint8_t',
-  'uint16': 'uint16_t',
-  'uint32': 'uint32_t',
-  'uint64': 'uint64_t',
-  'float32': 'float',
-  'bool': 'bool',
-  'fence_vertex': 'fence_vertex'}
-
-# Function to print a standard ros type
-def print_field_def(field):
-  type = field.type
-  # detect embedded types
-  sl_pos = type.find('/')
-  type_appendix = ''
-  type_prefix = ''
-  if (sl_pos >= 0):
-    type = type[sl_pos + 1:]
-    type_prefix = 'struct '
-    type_appendix = '_s'
-
-  # detect arrays
-  a_pos = type.find('[')
-  array_size = ''
-  if (a_pos >= 0):
-    # field is array
-    array_size = type[a_pos:]
-    type = type[:a_pos]
-
-  if type in type_map:
-    # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type]
-  else:
-    raise Exception("Type {0} not supported, add to to template file!".format(type))
-
-  print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
-
-}
-#ifdef __cplusplus
-@#class @(spec.short_name)_s {
-struct __EXPORT @(spec.short_name)_s {
-@#public:
-#else
-struct @(spec.short_name)_s {
-#endif
-@{
-# loop over all fields and print the type and name
-for field in spec.parsed_fields():
-  if (not field.is_header):
-    print_field_def(field)
-}@
-};
-
-/**
- * @@}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(@(spec.short_name));
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 622641617f..f16095c978 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -77,10 +77,13 @@ for field in spec.parsed_fields():
       print('#include <uORB/topics/%s.h>'%(name))
 
 }@
-@# Constants
+
+@# Constants c style
+#ifndef __cplusplus
 @[for constant in spec.constants]@
 #define @(constant.name) @(int(constant.val))
 @[end for]
+#endif
 
 /**
  * @@addtogroup topics
@@ -101,8 +104,10 @@ type_map = {'int8': 'int8_t',
   'uint32': 'uint32_t',
   'uint64': 'uint64_t',
   'float32': 'float',
+  'float64': 'double',
   'bool': 'bool',
-  'fence_vertex': 'fence_vertex'}
+  'fence_vertex': 'fence_vertex',
+  'position_setpoint': 'position_setpoint'}
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -146,6 +151,20 @@ for field in spec.parsed_fields():
   if (not field.is_header):
     print_field_def(field)
 }@
+#ifdef __cplusplus
+@# Constants again c++-ified
+@{
+for constant in spec.constants:
+  type = constant.type
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
+}
+#endif
 };
 
 /**

From 2d124852c1881d5b993b3c2ec9f7a79e1e03da1b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 07:58:42 +0100
Subject: [PATCH 387/416] propagate uorb contants change through all
 modules/drivers

---
 src/drivers/blinkm/blinkm.cpp                 |  12 +-
 src/drivers/px4fmu/fmu.cpp                    |  16 +-
 src/modules/commander/commander.cpp           | 296 +++++++++---------
 src/modules/commander/commander_helper.cpp    |  12 +-
 .../commander/state_machine_helper.cpp        | 190 +++++------
 .../ekf_att_pos_estimator_main.cpp            |   4 +-
 .../fw_pos_control_l1_main.cpp                |  18 +-
 src/modules/mavlink/mavlink_main.cpp          |   2 +-
 src/modules/mavlink/mavlink_messages.cpp      |  56 ++--
 src/modules/mavlink/mavlink_receiver.cpp      |   6 +-
 .../mc_pos_control/mc_pos_control_main.cpp    |   8 +-
 src/modules/navigator/loiter.cpp              |   2 +-
 src/modules/navigator/mission.cpp             |   4 +-
 src/modules/navigator/mission_block.cpp       |  10 +-
 src/modules/navigator/navigator_main.cpp      |  30 +-
 src/modules/sensors/sensors.cpp               |  86 ++---
 16 files changed, 376 insertions(+), 376 deletions(-)

diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 6b14f5945a..d0253a8d10 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -571,7 +571,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -595,7 +595,7 @@ BlinkM::led()
 					led_color_8 = LED_BLUE;
 					led_blink = LED_BLINK;
 
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;
@@ -647,14 +647,14 @@ BlinkM::led()
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
+							if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
 							/* TODO: add other Auto modes */
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
+							else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
 							else
 								led_color_4 = LED_OFF;
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 2552e7e6a6..db6b34f999 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -142,11 +142,11 @@ private:
 
 	uint32_t	_groups_required;
 	uint32_t	_groups_subscribed;
-	int		_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
-	actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
-	orb_id_t	_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
+	int		_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	orb_id_t	_actuator_output_topic;
-	pollfd	_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
@@ -514,7 +514,7 @@ PX4FMU::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	_poll_fds_num = 0;
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
@@ -591,7 +591,7 @@ PX4FMU::task_main()
 			}
 
 			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					orb_set_interval(_control_subs[i], update_rate_in_ms);
 				}
@@ -618,7 +618,7 @@ PX4FMU::task_main()
 
 			/* get controls for required topics */
 			unsigned poll_id = 0;
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -751,7 +751,7 @@ PX4FMU::task_main()
 
 	}
 
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a1c4e205d4..8f491acae7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -368,31 +368,31 @@ void print_status()
 	const char *armed_str;
 
 	switch (state.arming_state) {
-	case ARMING_STATE_INIT:
+	case vehicle_status_s::ARMING_STATE_INIT:
 		armed_str = "INIT";
 		break;
 
-	case ARMING_STATE_STANDBY:
+	case vehicle_status_s::ARMING_STATE_STANDBY:
 		armed_str = "STANDBY";
 		break;
 
-	case ARMING_STATE_ARMED:
+	case vehicle_status_s::ARMING_STATE_ARMED:
 		armed_str = "ARMED";
 		break;
 
-	case ARMING_STATE_ARMED_ERROR:
+	case vehicle_status_s::ARMING_STATE_ARMED_ERROR:
 		armed_str = "ARMED_ERROR";
 		break;
 
-	case ARMING_STATE_STANDBY_ERROR:
+	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR:
 		armed_str = "STANDBY_ERROR";
 		break;
 
-	case ARMING_STATE_REBOOT:
+	case vehicle_status_s::ARMING_STATE_REBOOT:
 		armed_str = "REBOOT";
 		break;
 
-	case ARMING_STATE_IN_AIR_RESTORE:
+	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE:
 		armed_str = "IN_AIR_RESTORE";
 		break;
 
@@ -415,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
+	arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
 					     true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
@@ -452,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
 			/* set HIL state */
-			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
 			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
 
 			// Transition the arming state
@@ -462,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
-					main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
-					main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
-					main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
 					/* OFFBOARD */
-					main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
 				}
 
 			} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
-						main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
-						main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
 					}
 				}
 			}
@@ -525,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 				// Flick to inair restore first if this comes from an onboard system
 				if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
-					status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+					status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
 				}
 
 				transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -550,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			unsigned int mav_goto = (cmd->param1 + 0.5f);
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "Pause mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+				status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "Continue mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -668,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 	case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
 			transition_result_t res = TRANSITION_DENIED;
-			static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
+			static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL;
 
-			if (status_local->main_state != MAIN_STATE_OFFBOARD) {
+			if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) {
 				main_state_pre_offboard = status_local->main_state;
 			}
 
 			if (cmd->param1 > 0.5f) {
-				res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 
 				if (res == TRANSITION_DENIED) {
 					print_reject_mode(status_local, "OFFBOARD");
@@ -798,41 +798,41 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
-	const char *main_states_str[MAIN_STATE_MAX];
-	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
-	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
-	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
-	main_states_str[MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
-	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
-	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
-	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
-	main_states_str[MAIN_STATE_OFFBOARD]			= "OFFBOARD";
+	const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
+	main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
+	main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL]			= "ALTCTL";
+	main_states_str[vehicle_status_s::MAIN_STATE_POSCTL]			= "POSCTL";
+	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
+	main_states_str[vehicle_status_s::MAIN_STATE_ACRO]			= "ACRO";
+	main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD]			= "OFFBOARD";
 
-	const char *arming_states_str[ARMING_STATE_MAX];
-	arming_states_str[ARMING_STATE_INIT]			= "INIT";
-	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
-	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
-	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
-	arming_states_str[ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
-	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
-	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
+	const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
+	arming_states_str[vehicle_status_s::ARMING_STATE_INIT]			= "INIT";
+	arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY]			= "STANDBY";
+	arming_states_str[vehicle_status_s::ARMING_STATE_ARMED]			= "ARMED";
+	arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
+	arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
+	arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT]			= "REBOOT";
+	arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
 
-	const char *nav_states_str[NAVIGATION_STATE_MAX];
-	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
-	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
-	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
-	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
-	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
-	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
-	nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
-	nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
-	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
-	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
-	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
-	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
-	nav_states_str[NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
+	const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL]			= "POSCTL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO]			= "ACRO";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND]			= "LAND";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND]		= "DESCEND";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -853,10 +853,10 @@ int commander_thread_main(int argc, char *argv[])
 	status.condition_landed = true;	// initialize to safe value
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
-	status.main_state = MAIN_STATE_MANUAL;
-	status.nav_state = NAVIGATION_STATE_MANUAL;
-	status.arming_state = ARMING_STATE_INIT;
-	status.hil_state = HIL_STATE_OFF;
+	status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
+	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
+	status.hil_state = vehicle_status_s::HIL_STATE_OFF;
 	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
@@ -869,7 +869,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.data_link_lost = true;
 
 	/* set battery warning flag */
-	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+	status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE;
 	status.condition_battery_voltage_valid = false;
 
 	// XXX for now just set sensors as initialized
@@ -1143,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* disable manual override for all systems that rely on electronic stabilization */
-				if (status.system_type == VEHICLE_TYPE_COAXIAL ||
-				    status.system_type == VEHICLE_TYPE_HELICOPTER ||
-				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
-				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    status.system_type == VEHICLE_TYPE_OCTOROTOR ||
-				    (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
-				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
+				if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
+				    status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
+				    (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
+				    (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
 
 					status.is_rotary_wing = true;
 
@@ -1159,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* set vehicle_status.is_vtol flag */
-				status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
-						 (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
+				status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
+						 (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
 
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
@@ -1310,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
-			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
-				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
-								   ARMING_STATE_STANDBY_ERROR);
+			if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
+				arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
+								   vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
 
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
 						true /* fRunPreArmChecks */, mavlink_fd)) {
@@ -1344,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
 			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 
 			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
-			if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
+			if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
 			}
 		}
@@ -1516,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
-			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
 		} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
@@ -1524,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[])
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
-			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
+				arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
 								     mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
@@ -1535,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
+				arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
 								     mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
@@ -1549,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
+		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
+			arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
 							     mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
@@ -1662,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[])
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
-			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
+			    (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
+			    (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
-					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
-									   ARMING_STATE_STANDBY_ERROR);
+					arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
+									   vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
 					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
 									     mavlink_fd);
 
@@ -1688,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-			if (status.arming_state == ARMING_STATE_STANDBY &&
+			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY &&
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 
@@ -1696,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[])
 					 * for being in manual mode only applies to manual arming actions.
 					 * the system can be armed in auto if armed via the GCS.
 					 */
-					if (status.main_state != MAIN_STATE_MANUAL) {
+					if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
+						arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
 										     mavlink_fd);
 
 						if (arming_ret == TRANSITION_CHANGED) {
@@ -1719,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			if (arming_ret == TRANSITION_CHANGED) {
-				if (status.arming_state == ARMING_STATE_ARMED) {
+				if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "ARMED by RC");
 
 				} else {
@@ -1857,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
 			/* At this point the data link and the gps system have been checked
 			 * If  we are not in a manual (RC stick controlled mode)
 			 * and both failed we want to terminate the flight */
-			if (status.main_state != MAIN_STATE_MANUAL &&
-			    status.main_state != MAIN_STATE_ACRO &&
-			    status.main_state != MAIN_STATE_ALTCTL &&
-			    status.main_state != MAIN_STATE_POSCTL &&
+			if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
 			    ((status.data_link_lost && status.gps_failure) ||
 			     (status.data_link_lost_cmd && status.gps_failure_cmd))) {
 				armed.force_failsafe = true;
@@ -1881,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[])
 			/* At this point the rc signal and the gps system have been checked
 			 * If we are in manual (controlled with RC):
 			 * if both failed we want to terminate the flight */
-			if ((status.main_state == MAIN_STATE_ACRO ||
-			     status.main_state == MAIN_STATE_MANUAL ||
-			     status.main_state == MAIN_STATE_ALTCTL ||
-			     status.main_state == MAIN_STATE_POSCTL) &&
+			if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
 			    ((status.rc_signal_lost && status.gps_failure) ||
 			     (status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
 				armed.force_failsafe = true;
@@ -1976,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[])
 			set_tune(TONE_ARMING_WARNING_TUNE);
 			arm_tune_played = true;
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+		} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
+		} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -2072,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 		bool set_normal_color = false;
 
 		/* set mode */
-		if (status_local->arming_state == ARMING_STATE_ARMED) {
+		if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
-		} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
 			set_normal_color = true;
 
@@ -2091,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 
 		if (set_normal_color) {
 			/* set color */
-			if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
+			if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+				/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
 
 			} else {
 				if (status_local->condition_local_position_valid) {
@@ -2149,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 
 	/* if offboard is set allready by a mavlink command, abort */
 	if (status.offboard_control_set_by_command) {
-		return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+		return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 	}
 
 	/* offboard switch overrides main switch */
-	if (sp_man->offboard_switch == SWITCH_POS_ON) {
-		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+	if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 
 		if (res == TRANSITION_DENIED) {
 			print_reject_mode(status_local, "OFFBOARD");
@@ -2166,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 
 	/* offboard switched off or denied, check main mode switch */
 	switch (sp_man->mode_switch) {
-	case SWITCH_POS_NONE:
+	case manual_control_setpoint_s::SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
 		break;
 
-	case SWITCH_POS_OFF:		// MANUAL
-		if (sp_man->acro_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_ACRO);
+	case manual_control_setpoint_s::SWITCH_POS_OFF:		// MANUAL
+		if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
 
 		} else {
-			res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		}
 
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case SWITCH_POS_MIDDLE:		// ASSIST
-		if (sp_man->posctl_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+	case manual_control_setpoint_s::SWITCH_POS_MIDDLE:		// ASSIST
+		if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2193,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		}
 
 		// fallback to ALTCTL
-		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
 		}
 
-		if (sp_man->posctl_switch != SWITCH_POS_ON) {
+		if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
 			print_reject_mode(status_local, "ALTCTL");
 		}
 
 		// fallback to MANUAL
-		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case SWITCH_POS_ON:			// AUTO
-		if (sp_man->return_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
+	case manual_control_setpoint_s::SWITCH_POS_ON:			// AUTO
+		if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2219,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "AUTO_RTL");
 
 			// fallback to LOITER if home position not set
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;  // changed successfully or already in this state
 			}
 
-		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+		} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2235,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "AUTO_LOITER");
 
 		} else {
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
@@ -2244,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			print_reject_mode(status_local, "AUTO_MISSION");
 
 			// fallback to LOITER if home position not set
-			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;  // changed successfully or already in this state
@@ -2252,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		}
 
 		// fallback to POSCTL
-		res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;  // changed successfully or already in this state
 		}
 
 		// fallback to ALTCTL
-		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
 		// fallback to MANUAL
-		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
+		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -2283,11 +2283,11 @@ set_control_mode()
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
 	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
-	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+	control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
 	switch (status.nav_state) {
-	case NAVIGATION_STATE_MANUAL:
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
@@ -2299,7 +2299,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ALTCTL:
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2311,7 +2311,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_POSCTL:
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2323,12 +2323,12 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_AUTO_MISSION:
-	case NAVIGATION_STATE_AUTO_LOITER:
-	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_RCRECOVER:
-	case NAVIGATION_STATE_AUTO_RTGS:
-	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
@@ -2340,7 +2340,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2352,7 +2352,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ACRO:
+	case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_rates_enabled = true;
@@ -2365,7 +2365,7 @@ set_control_mode()
 		break;
 
 
-	case NAVIGATION_STATE_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
@@ -2378,7 +2378,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_DESCEND:
+	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 		/* TODO: check if this makes sense */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
@@ -2391,7 +2391,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_TERMINATION:
+	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -2404,7 +2404,7 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = true;
 		break;
 
-	case NAVIGATION_STATE_OFFBOARD:
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
 		control_mode.flag_control_offboard_enabled = true;
@@ -2603,7 +2603,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
 						true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
@@ -2667,7 +2667,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+				arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				break;
 			}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 2022e99fb3..8a44511008 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -72,16 +72,16 @@ static const int ERROR = -1;
 
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
-	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
-		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+	return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
-	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
-	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+	return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER)
+	       || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
 }
 
 static int buzzer = -1;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 465f9cdc52..40da9c77b1 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -76,19 +76,19 @@ static const int ERROR = -1;
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
-static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
+static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
 	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
-	{ /* ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
-	{ /* ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
-	{ /* ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
-	{ /* ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
-	{ /* ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
-	{ /* ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
-	{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
+	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* vehicle_status_s::ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
-static const char * const state_names[ARMING_STATE_MAX] = {
+static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_INIT",
 	"ARMING_STATE_STANDBY",
 	"ARMING_STATE_ARMED",
@@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
 {
 	// Double check that our static arrays are still valid
-	ASSERT(ARMING_STATE_INIT == 0);
-	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
+	ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
 	arming_state_t current_arming_state = status->arming_state;
@@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
@@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		irqstate_t flags = irqsave();
 
 		/* enforce lockdown in HIL */
-		if (status->hil_state == HIL_STATE_ON) {
+		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 			armed->lockdown = true;
 
 		} else {
@@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
-			if (new_arming_state == ARMING_STATE_ARMED) {
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
 				//      Do not perform pre-arm checks if coming from in air restore
-				//      Allow if HIL_STATE_ON
-				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
-					status->hil_state == HIL_STATE_OFF) {
+				//      Allow if vehicle_status_s::HIL_STATE_ON
+				if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
@@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 				}
 
-			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
-				new_arming_state = ARMING_STATE_STANDBY_ERROR;
+			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
+				new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 			}
 		}
 
 		// HIL can always go to standby
-		if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+		if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			valid_transition = true;
 		}
 
 		/* Sensors need to be initialized for STANDBY state */
-		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
 			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
 			feedback_provided = true;
 			valid_transition = false;
@@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		// Finish up the state transition
 		if (valid_transition) {
-			armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
-			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
+			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
 		}
@@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
-	case MAIN_STATE_MANUAL:
-	case MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_ACRO:
 		ret = TRANSITION_CHANGED;
 		break;
 
-	case MAIN_STATE_ALTCTL:
+	case vehicle_status_s::MAIN_STATE_ALTCTL:
 		/* need at minimum altitude estimate */
 		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
@@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
-	case MAIN_STATE_POSCTL:
+	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
 		    status->condition_global_position_valid) {
@@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
-	case MAIN_STATE_AUTO_LOITER:
+	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
 
-	case MAIN_STATE_AUTO_MISSION:
-	case MAIN_STATE_AUTO_RTL:
+	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
+	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
 		/* need global position and home position */
 		if (status->condition_global_position_valid && status->condition_home_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
 
-	case MAIN_STATE_OFFBOARD:
+	case vehicle_status_s::MAIN_STATE_OFFBOARD:
 
 		/* need offboard signal */
 		if (!status->offboard_control_signal_lost) {
@@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 		break;
 
-	case MAIN_STATE_MAX:
+	case vehicle_status_s::MAIN_STATE_MAX:
 	default:
 		break;
 	}
@@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 
 	} else {
 		switch (new_state) {
-		case HIL_STATE_OFF:
+		case vehicle_status_s::HIL_STATE_OFF:
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
 			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
 			ret = TRANSITION_DENIED;
 			break;
 
-		case HIL_STATE_ON:
-			if (current_status->arming_state == ARMING_STATE_INIT
-			    || current_status->arming_state == ARMING_STATE_STANDBY
-			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
+		case vehicle_status_s::HIL_STATE_ON:
+			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
+			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
+			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
 
 				/* Disable publication of all attached sensors */
 				/* list directory */
@@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
-	bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
+	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 	status->failsafe = false;
 
 	/* evaluate main state to decide in normal (non-failsafe) mode */
 	switch (status->main_state) {
-	case MAIN_STATE_ACRO:
-	case MAIN_STATE_MANUAL:
-	case MAIN_STATE_ALTCTL:
-	case MAIN_STATE_POSCTL:
+	case vehicle_status_s::MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_ALTCTL:
+	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
 		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		} else {
 			switch (status->main_state) {
-			case MAIN_STATE_ACRO:
-				status->nav_state = NAVIGATION_STATE_ACRO;
+			case vehicle_status_s::MAIN_STATE_ACRO:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
 				break;
 
-			case MAIN_STATE_MANUAL:
-				status->nav_state = NAVIGATION_STATE_MANUAL;
+			case vehicle_status_s::MAIN_STATE_MANUAL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
 
-			case MAIN_STATE_ALTCTL:
-				status->nav_state = NAVIGATION_STATE_ALTCTL;
+			case vehicle_status_s::MAIN_STATE_ALTCTL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 				break;
 
-			case MAIN_STATE_POSCTL:
-				status->nav_state = NAVIGATION_STATE_POSCTL;
+			case vehicle_status_s::MAIN_STATE_POSCTL:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 				break;
 
 			default:
-				status->nav_state = NAVIGATION_STATE_MANUAL;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
 			}
 		}
 		break;
 
-	case MAIN_STATE_AUTO_MISSION:
+	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
 
 		/* go into failsafe
 		 * - if commanded to do so
@@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 		/* first look at the commands */
 		if (status->engine_failure_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->data_link_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 		} else if (status->gps_failure_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 
 		/* finished handling commands which have priority, now handle failures */
 		} else if (status->gps_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->engine_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
 		/* datalink loss enabled:
 		 * check for datalink lost: this should always trigger RTGS */
@@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* datalink loss disabled:
@@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 		} else if (!stay_in_failsafe){
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
 
-	case MAIN_STATE_AUTO_LOITER:
+	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
 		/* go into failsafe on a engine failure */
 		if (status->engine_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		/* also go into failsafe if just datalink is lost */
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* go into failsafe if RC is lost and datalink loss is not set up */
@@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
 		/* don't bother if RC is lost if datalink is connected */
 		} else if (status->rc_signal_lost) {
 
 			/* this mode is ok, we don't need RC for loitering */
-			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 		} else {
 			/* everything is perfect */
-			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 		}
 		break;
 
-	case MAIN_STATE_AUTO_RTL:
+	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
 		/* require global position and home, also go into failsafe on an engine failure */
 
 		if (status->engine_failure) {
-			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if ((!status->condition_global_position_valid ||
 					!status->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 		} else {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 		}
 		break;
 
-	case MAIN_STATE_OFFBOARD:
+	case vehicle_status_s::MAIN_STATE_OFFBOARD:
 		/* require offboard control, otherwise stay where you are */
 		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
 			status->failsafe = true;
 
-			status->nav_state = NAVIGATION_STATE_POSCTL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
 			status->failsafe = true;
 
 			if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 		} else {
-			status->nav_state = NAVIGATION_STATE_OFFBOARD;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}
 	default:
 		break;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index d51075b8c5..e33691b0c4 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -817,7 +817,7 @@ FixedwingEstimator::task_main()
 		if (fds[1].revents & POLLIN) {
 
 			/* check vehicle status for changes to publication state */
-			bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
+			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
 			vehicle_status_poll();
 
 			bool accel_updated;
@@ -826,7 +826,7 @@ FixedwingEstimator::task_main()
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
-			if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
+			if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 74249c9c53..0bdc285e70 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -816,7 +816,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
+	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -963,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
+		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -974,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
 			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
@@ -987,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
 			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1140,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						ground_speed);
 			}
 
-		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
 			if (launchDetector.launchDetectionEnabled() &&
@@ -1235,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 
 		/* reset landing state */
-		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
+		if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) {
 			reset_landing_state();
 		}
 
 		/* reset takeoff/launch state */
-		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
+		if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 			reset_takeoff_state();
 		}
 
@@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
-			pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
@@ -1341,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
 	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
-				pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+				pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 9e4ab00df2..676b65adc1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
-			set_hil_enabled(status.hil_state == HIL_STATE_ON);
+			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
 		}
 
 		/* check for requested subscriptions */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 6765100c70..6642fb2ac6 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	*mavlink_custom_mode = 0;
 
 	/* HIL */
-	if (status->hil_state == HIL_STATE_ON) {
+	if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
 	/* arming state */
-	if (status->arming_state == ARMING_STATE_ARMED
-	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+	if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
+	    || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	switch (status->nav_state) {
 
-		case NAVIGATION_STATE_MANUAL:
+		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 			break;
 
-		case NAVIGATION_STATE_ACRO:
+		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 			break;
 
-		case NAVIGATION_STATE_ALTCTL:
+		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
 			break;
 
-		case NAVIGATION_STATE_POSCTL:
+		case vehicle_status_s::NAVIGATION_STATE_POSCTL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 			break;
 
-		case NAVIGATION_STATE_AUTO_MISSION:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
 			break;
 
-		case NAVIGATION_STATE_AUTO_LOITER:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 			break;
 
-		case NAVIGATION_STATE_AUTO_RTL:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			/* fallthrough */
-		case NAVIGATION_STATE_AUTO_RCRECOVER:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 			break;
 
-		case NAVIGATION_STATE_LAND:
-		case NAVIGATION_STATE_AUTO_LANDENGFAIL:
-		case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+		case vehicle_status_s::NAVIGATION_STATE_LAND:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 			/* fallthrough */
-		case NAVIGATION_STATE_DESCEND:
+		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 			break;
 
-		case NAVIGATION_STATE_AUTO_RTGS:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
 			break;
 
-		case NAVIGATION_STATE_TERMINATION:
+		case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 			break;
 
-		case NAVIGATION_STATE_OFFBOARD:
+		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
 			break;
 
-		case NAVIGATION_STATE_MAX:
+		case vehicle_status_s::NAVIGATION_STATE_MAX:
 			/* this is an unused case, ignore */
 			break;
 
@@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	*mavlink_custom_mode = custom_mode.data;
 
 	/* set system state */
-	if (status->arming_state == ARMING_STATE_INIT
-	    || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
-	    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+	if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT
+	    || status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE
+	    || status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {	// TODO review
 		*mavlink_state = MAV_STATE_UNINIT;
 
-	} else if (status->arming_state == ARMING_STATE_ARMED) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 		*mavlink_state = MAV_STATE_ACTIVE;
 
-	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 		*mavlink_state = MAV_STATE_CRITICAL;
 
-	} else if (status->arming_state == ARMING_STATE_STANDBY) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 		*mavlink_state = MAV_STATE_STANDBY;
 
-	} else if (status->arming_state == ARMING_STATE_REBOOT) {
+	} else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) {
 		*mavlink_state = MAV_STATE_POWEROFF;
 
 	} else {
@@ -1438,7 +1438,7 @@ protected:
 		updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
 		updated |= _status_sub->update(&_status_time, &status);
 
-		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
+		if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
@@ -2205,7 +2205,7 @@ protected:
 		msg.param2 = 0;
 		msg.param3 = 0;
 		/* set camera capture ON/OFF depending on arming state */
-		msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0;
+		msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0;
 		msg.param5 = 0;
 		msg.param6 = 0;
 		msg.param7 = 0;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 2e28a6d2c5..b1ba91cac9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -617,7 +617,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.previous.valid = false;
 					pos_sp_triplet.next.valid = false;
 					pos_sp_triplet.current.valid = true;
-					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
+					pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
 
 					/* set the local pos values if the setpoint type is 'local pos' and none
 					 * of the local pos fields is set to 'ignore' */
@@ -986,10 +986,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 
 	} else if (tsync.tc1 > 0) {
 
-		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; 
+		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
 		int64_t dt = _time_offset - offset_ns;
 
-		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew 
+		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
 			_time_offset = offset_ns;
 			warnx("[timesync] Hard setting offset.");
 		} else {
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 60682fb8ee..b9692ffbfc 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -768,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt)
 		/* by default use current setpoint as is */
 		math::Vector<3> pos_sp_s = curr_sp_s;
 
-		if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
 			/* follow "previous - current" line */
 			math::Vector<3> prev_sp;
 			map_projection_project(&_ref_pos,
@@ -998,7 +998,7 @@ MulticopterPositionControl::task_main()
 			}
 
 
-			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
 				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
@@ -1037,7 +1037,7 @@ MulticopterPositionControl::task_main()
 				}
 
 				/* use constant descend rate when landing, ignore altitude setpoint */
-				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 					_vel_sp(2) = _params.land_speed;
 				}
 
@@ -1124,7 +1124,7 @@ MulticopterPositionControl::task_main()
 
 					/* adjust limits for landing mode */
 					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
-					    _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+					    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 						/* limit max tilt and min lift when landing */
 						tilt_max = _params.tilt_max_land;
 
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f827e70c96..a744d58cf0 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -82,7 +82,7 @@ Loiter::on_activation()
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 
-	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
 	_navigator->set_position_setpoint_triplet_updated();
 }
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9b0a092dad..b87bebd0cf 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -388,7 +388,7 @@ Mission::set_mission_items()
 		pos_sp_triplet->next.valid = false;
 
 		/* reuse setpoint for LOITER only if it's not IDLE */
-		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
 		set_mission_finished();
 
@@ -462,7 +462,7 @@ Mission::set_mission_items()
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
 	/* require takeoff after landing or idle */
-	if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_need_takeoff = true;
 	}
 
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 723caec7ce..c936489d5e 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -193,25 +193,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_IDLE:
-		sp->type = SETPOINT_TYPE_IDLE;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 		break;
 
 	case NAV_CMD_TAKEOFF:
-		sp->type = SETPOINT_TYPE_TAKEOFF;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 		break;
 
 	case NAV_CMD_LAND:
-		sp->type = SETPOINT_TYPE_LAND;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
 		break;
 
 	case NAV_CMD_LOITER_TIME_LIMIT:
 	case NAV_CMD_LOITER_TURN_COUNT:
 	case NAV_CMD_LOITER_UNLIMITED:
-		sp->type = SETPOINT_TYPE_LOITER;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 		break;
 
 	default:
-		sp->type = SETPOINT_TYPE_POSITION;
+		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		break;
 	}
 }
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 3f7670ec4a..e35b0bd6a1 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -217,7 +217,7 @@ Navigator::vehicle_status_update()
 {
 	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
 		/* in case the commander is not be running */
-		_vstatus.arming_state = ARMING_STATE_STANDBY;
+		_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
 	}
 }
 
@@ -419,25 +419,25 @@ Navigator::task_main()
 
 		/* Do stuff according to navigation state set by commander */
 		switch (_vstatus.nav_state) {
-			case NAVIGATION_STATE_MANUAL:
-			case NAVIGATION_STATE_ACRO:
-			case NAVIGATION_STATE_ALTCTL:
-			case NAVIGATION_STATE_POSCTL:
-			case NAVIGATION_STATE_LAND:
-			case NAVIGATION_STATE_TERMINATION:
-			case NAVIGATION_STATE_OFFBOARD:
+			case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+			case vehicle_status_s::NAVIGATION_STATE_ACRO:
+			case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+			case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+			case vehicle_status_s::NAVIGATION_STATE_LAND:
+			case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
+			case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
 				break;
-			case NAVIGATION_STATE_AUTO_MISSION:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_mission;
 				break;
-			case NAVIGATION_STATE_AUTO_LOITER:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_loiter;
 				break;
-			case NAVIGATION_STATE_AUTO_RCRECOVER:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
 				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
@@ -445,11 +445,11 @@ Navigator::task_main()
 					_navigation_mode = &_rtl;
 				}
 				break;
-			case NAVIGATION_STATE_AUTO_RTL:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_rtl;
 				break;
-			case NAVIGATION_STATE_AUTO_RTGS:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
 				_pos_sp_triplet_published_invalid_once = false;
@@ -459,11 +459,11 @@ Navigator::task_main()
 					_navigation_mode = &_rtl;
 				}
 				break;
-			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_engineFailure;
 				break;
-			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_gpsFailure;
 				break;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6aa6b6bbda..82bb1eb8e2 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -813,28 +813,28 @@ Sensors::parameters_update()
 	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
 
 	/* update RC function mappings */
-	_rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
 
-	_rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
 
-	_rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
 
-	_rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
-	_rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
 
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+		_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
 
 	/* gyro offsets */
@@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced)
 		orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
 		/* update paramter handles to which the RC channels are mapped */
 		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-			if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+			if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 				/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 				 * or no request to map this channel to a param has been sent via mavlink
 				 */
@@ -1673,17 +1673,17 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi
 		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
 
 		if (on_inv ? value < on_th : value > on_th) {
-			return SWITCH_POS_ON;
+			return manual_control_setpoint_s::SWITCH_POS_ON;
 
 		} else if (mid_inv ? value < mid_th : value > mid_th) {
-			return SWITCH_POS_MIDDLE;
+			return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
 
 		} else {
-			return SWITCH_POS_OFF;
+			return manual_control_setpoint_s::SWITCH_POS_OFF;
 		}
 
 	} else {
-		return SWITCH_POS_NONE;
+		return manual_control_setpoint_s::SWITCH_POS_NONE;
 	}
 }
 
@@ -1694,14 +1694,14 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
 
 		if (on_inv ? value < on_th : value > on_th) {
-			return SWITCH_POS_ON;
+			return manual_control_setpoint_s::SWITCH_POS_ON;
 
 		} else {
-			return SWITCH_POS_OFF;
+			return manual_control_setpoint_s::SWITCH_POS_OFF;
 		}
 
 	} else {
-		return SWITCH_POS_NONE;
+		return manual_control_setpoint_s::SWITCH_POS_NONE;
 	}
 }
 
@@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc()
 {
 	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+		if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 			 * or no request to map this channel to a param has been sent via mavlink
 			 */
 			continue;
 		}
 
-		float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
+		float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
 		/* Check if the value has changed,
 		 * maybe we need to introduce a more aggressive limit here */
 		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
@@ -1847,24 +1847,24 @@ Sensors::rc_poll()
 			manual.timestamp = rc_input.timestamp_last_signal;
 
 			/* limit controls */
-			manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
-			manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
-			manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
-			manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
-			manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
-			manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
-			manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
-			manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
-			manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
-			manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
+			manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
+			manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
+			manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
+			manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
+			manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
+			manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
+			manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
+			manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
+			manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
+			manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
 
 			/* mode switches */
-			manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
-			manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
-			manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
-			manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
-			manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
-			manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+			manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+			manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+			manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+			manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+			manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {

From 719edf93e4c3994d3d61bff4435d5ac0760d912b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 08:01:22 +0100
Subject: [PATCH 388/416] ported more geo functions to cpp

---
 src/platforms/ros/geo.cpp | 93 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 93 insertions(+)

diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp
index 6fad681c99..04094de8bb 100644
--- a/src/platforms/ros/geo.cpp
+++ b/src/platforms/ros/geo.cpp
@@ -171,3 +171,96 @@ __EXPORT float _wrap_360(float bearing)
 
 	return bearing;
 }
+
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
+{
+	return ref->init_done;
+}
+
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
+{
+	return ref->timestamp;
+}
+
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+
+	ref->lat_rad = lat_0 * M_DEG_TO_RAD;
+	ref->lon_rad = lon_0 * M_DEG_TO_RAD;
+	ref->sin_lat = sin(ref->lat_rad);
+	ref->cos_lat = cos(ref->lat_rad);
+
+	ref->timestamp = timestamp;
+	ref->init_done = true;
+
+	return 0;
+}
+
+__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros());
+}
+
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	*ref_lat_rad = ref->lat_rad;
+	*ref_lon_rad = ref->lon_rad;
+
+	return 0;
+}
+
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double lat_rad = lat * M_DEG_TO_RAD;
+	double lon_rad = lon * M_DEG_TO_RAD;
+
+	double sin_lat = sin(lat_rad);
+	double cos_lat = cos(lat_rad);
+	double cos_d_lon = cos(lon_rad - ref->lon_rad);
+
+	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
+	double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
+
+	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
+	*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
+
+	return 0;
+}
+
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
+	double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
+	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_rad;
+	double lon_rad;
+
+	if (fabs(c) > DBL_EPSILON) {
+		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
+		lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+
+	} else {
+		lat_rad = ref->lat_rad;
+		lon_rad = ref->lon_rad;
+	}
+
+	*lat = lat_rad * 180.0 / M_PI;
+	*lon = lon_rad * 180.0 / M_PI;
+
+	return 0;
+}

From 6ba1912309fef5b7aa3fc2cda73a124b6b01a01f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 08:02:08 +0100
Subject: [PATCH 389/416] add angle conversion defines for ros

---
 src/platforms/px4_defines.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index c8e2cf290b..fa4e1398e2 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -135,6 +135,8 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #define M_IVLN10_F      0.43429448190325182765f /* 1 / log(10) */
 #define M_LOG2_E_F      _M_LN2_F
 #define M_INVLN2_F      1.4426950408889633870E0f  /* 1 / log(2) */
+#define M_DEG_TO_RAD 	0.01745329251994
+#define M_RAD_TO_DEG 	57.2957795130823
 
 #else
 /*

From 19155372810e2f70d273b7d06069748d1e01071b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 08:04:00 +0100
Subject: [PATCH 390/416] initial port of multiplatform version of
 mc_pos_control

---
 CMakeLists.txt                                |  15 +
 ROMFS/px4fmu_common/init.d/rc.mc_apps         |   8 +-
 makefiles/config_px4fmu-v2_multiplatform.mk   |   3 +-
 .../mc_pos_control.cpp                        | 947 ++++++++++++++++++
 .../mc_pos_control.h                          | 219 ++++
 .../mc_pos_control_main.cpp                   |  63 ++
 .../mc_pos_control_params.c                   | 212 ++++
 .../mc_pos_control_params.h                   |  61 ++
 .../mc_pos_control_start_nuttx.cpp            |  99 ++
 .../mc_pos_control_multiplatform/module.mk    |  43 +
 src/platforms/px4_includes.h                  |   8 +
 11 files changed, 1676 insertions(+), 2 deletions(-)
 create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
 create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.h
 create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
 create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
 create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
 create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
 create mode 100644 src/modules/mc_pos_control_multiplatform/module.mk

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f76dbbf418..6a368d78fc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -69,6 +69,11 @@ add_message_files(
   actuator_armed.msg
   parameter_update.msg
   vehicle_status.msg
+  vehicle_local_position.msg
+  position_setpoint.msg
+  position_setpoint_triplet.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_global_velocity_setpoint.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -180,6 +185,16 @@ target_link_libraries(mc_att_control
   px4
 )
 
+## MC Position Control
+add_executable(mc_pos_control
+	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
+add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_pos_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
 ## Attitude Estimator dummy
 add_executable(attitude_estimator
 	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 7b29fb3a7b..2ecc104df7 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -14,7 +14,13 @@ else
 	# try the multiplatform version
 	mc_att_control_m start
 fi
-mc_pos_control start
+
+if mc_pos_control start
+then
+else
+	# try the multiplatform version
+	mc_pos_control_m start
+fi
 
 #
 # Start Land Detector
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index 76edade4b8..29eb680960 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -89,7 +89,8 @@ MODULES		+= modules/position_estimator_inav
 MODULES		+= modules/mc_att_control_multiplatform
 MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
-MODULES		+= modules/mc_pos_control
+# MODULES		+= modules/mc_pos_control
+MODULES		+= modules/mc_pos_control_multiplatform
 MODULES 	+= modules/vtol_att_control
 
 #
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
new file mode 100644
index 0000000000..2685a99c32
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -0,0 +1,947 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control.cpp
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+#include "mc_pos_control_params.h"
+
+#define TILT_COS_MAX	0.7f
+#define SIGMA		0.000001f
+#define MIN_DIST	0.01f
+
+MulticopterPositionControl::MulticopterPositionControl() :
+
+	_task_should_exit(false),
+	_control_task(-1),
+	_mavlink_fd(-1),
+
+/* publications */
+	_att_sp_pub(nullptr),
+	_local_pos_sp_pub(nullptr),
+	_global_vel_sp_pub(nullptr),
+
+	_ref_alt(0.0f),
+	_ref_timestamp(0),
+
+	_reset_pos_sp(true),
+	_reset_alt_sp(true),
+	_mode_auto(false),
+	_thrust_int(),
+	_R()
+{
+	memset(&_ref_pos, 0, sizeof(_ref_pos));
+
+	/*
+	 * Do subscriptions
+	 */
+	_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
+	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
+	_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
+	_parameter_update = _n.subscribe<px4_parameter_update>(
+			&MulticopterPositionControl::handle_parameter_update, this, 1000);
+	_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
+	_armed = _n.subscribe<px4_actuator_armed>(0);
+	_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
+	_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
+
+
+
+	_params.pos_p.zero();
+	_params.vel_p.zero();
+	_params.vel_i.zero();
+	_params.vel_d.zero();
+	_params.vel_max.zero();
+	_params.vel_ff.zero();
+	_params.sp_offs_max.zero();
+
+	_pos.zero();
+	_pos_sp.zero();
+	_vel.zero();
+	_vel_sp.zero();
+	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
+
+	_params_handles.thr_min		= PX4_PARAM_INIT(MPC_THR_MIN);
+	_params_handles.thr_max		= PX4_PARAM_INIT(MPC_THR_MAX);
+	_params_handles.z_p		= PX4_PARAM_INIT(MPC_Z_P);
+	_params_handles.z_vel_p		= PX4_PARAM_INIT(MPC_Z_VEL_P);
+	_params_handles.z_vel_i		= PX4_PARAM_INIT(MPC_Z_VEL_I);
+	_params_handles.z_vel_d		= PX4_PARAM_INIT(MPC_Z_VEL_D);
+	_params_handles.z_vel_max	= PX4_PARAM_INIT(MPC_Z_VEL_MAX);
+	_params_handles.z_ff		= PX4_PARAM_INIT(MPC_Z_FF);
+	_params_handles.xy_p		= PX4_PARAM_INIT(MPC_XY_P);
+	_params_handles.xy_vel_p	= PX4_PARAM_INIT(MPC_XY_VEL_P);
+	_params_handles.xy_vel_i	= PX4_PARAM_INIT(MPC_XY_VEL_I);
+	_params_handles.xy_vel_d	= PX4_PARAM_INIT(MPC_XY_VEL_D);
+	_params_handles.xy_vel_max	= PX4_PARAM_INIT(MPC_XY_VEL_MAX);
+	_params_handles.xy_ff		= PX4_PARAM_INIT(MPC_XY_FF);
+	_params_handles.tilt_max_air	= PX4_PARAM_INIT(MPC_TILTMAX_AIR);
+	_params_handles.land_speed	= PX4_PARAM_INIT(MPC_LAND_SPEED);
+	_params_handles.tilt_max_land	= PX4_PARAM_INIT(MPC_TILTMAX_LND);
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	_R.identity();
+}
+
+MulticopterPositionControl::~MulticopterPositionControl()
+{
+}
+
+int
+MulticopterPositionControl::parameters_update()
+{
+	PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min);
+	PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max);
+	PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air);
+	_params.tilt_max_air = math::radians(_params.tilt_max_air);
+	PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed);
+	PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land);
+	_params.tilt_max_land = math::radians(_params.tilt_max_land);
+
+	float v;
+	PX4_PARAM_GET(_params_handles.xy_p, &v);
+	_params.pos_p(0) = v;
+	_params.pos_p(1) = v;
+	PX4_PARAM_GET(_params_handles.z_p, &v);
+	_params.pos_p(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_p, &v);
+	_params.vel_p(0) = v;
+	_params.vel_p(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_p, &v);
+	_params.vel_p(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_i, &v);
+	_params.vel_i(0) = v;
+	_params.vel_i(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_i, &v);
+	_params.vel_i(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_d, &v);
+	_params.vel_d(0) = v;
+	_params.vel_d(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_d, &v);
+	_params.vel_d(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_vel_max, &v);
+	_params.vel_max(0) = v;
+	_params.vel_max(1) = v;
+	PX4_PARAM_GET(_params_handles.z_vel_max, &v);
+	_params.vel_max(2) = v;
+	PX4_PARAM_GET(_params_handles.xy_ff, &v);
+	v = math::constrain(v, 0.0f, 1.0f);
+	_params.vel_ff(0) = v;
+	_params.vel_ff(1) = v;
+	PX4_PARAM_GET(_params_handles.z_ff, &v);
+	v = math::constrain(v, 0.0f, 1.0f);
+	_params.vel_ff(2) = v;
+
+	_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
+
+	return OK;
+}
+
+
+float
+MulticopterPositionControl::scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+void
+MulticopterPositionControl::update_ref()
+{
+	if (_local_pos->data().ref_timestamp != _ref_timestamp) {
+		double lat_sp, lon_sp;
+		float alt_sp = 0.0f;
+
+		if (_ref_timestamp != 0) {
+			/* calculate current position setpoint in global frame */
+			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
+			alt_sp = _ref_alt - _pos_sp(2);
+		}
+
+		/* update local projection reference */
+		map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon);
+		_ref_alt = _local_pos->data().ref_alt;
+
+		if (_ref_timestamp != 0) {
+			/* reproject position setpoint to new reference */
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			_pos_sp(2) = -(alt_sp - _ref_alt);
+		}
+
+		_ref_timestamp = _local_pos->data().ref_timestamp;
+	}
+}
+
+void
+MulticopterPositionControl::reset_pos_sp()
+{
+	if (_reset_pos_sp) {
+		_reset_pos_sp = false;
+		/* shift position setpoint to make attitude setpoint continuous */
+		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
+				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
+				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
+
+		//XXX: port this once a mavlink like interface is available
+		// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
+		PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
+	}
+}
+
+void
+MulticopterPositionControl::reset_alt_sp()
+{
+	if (_reset_alt_sp) {
+		_reset_alt_sp = false;
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
+
+		//XXX: port this once a mavlink like interface is available
+		// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
+		PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2));
+	}
+}
+
+void
+MulticopterPositionControl::limit_pos_sp_offset()
+{
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual_control_sp->data().x;
+		_sp_move_rate(1) = _manual_control_sp->data().y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
+	math::Matrix<3, 3> R_yaw_sp;
+	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* move position setpoint */
+	_pos_sp += _sp_move_rate * dt;
+
+	/* check if position setpoint is too far from actual position */
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode->data().flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode->data().flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	if (_pos_sp_triplet->data().current.valid) {
+		if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet->data().current.x;
+			_pos_sp(1) = _pos_sp_triplet->data().current.y;
+		} else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet->data().current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet->data().current.vy;
+		}
+
+		if (_pos_sp_triplet->data().current.yaw_valid) {
+			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
+		} else if (_pos_sp_triplet->data().current.yawspeed_valid) {
+			_att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt;
+		}
+
+		if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet->data().current.z;
+		} else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet->data().current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
+bool
+MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+		const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
+{
+	/* project center of sphere on line */
+	/* normalized AB */
+	math::Vector<3> ab_norm = line_b - line_a;
+	ab_norm.normalize();
+	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	float cd_len = (sphere_c - d).length();
+
+	/* we have triangle CDX with known CD and CX = R, find DX */
+	if (sphere_r > cd_len) {
+		/* have two roots, select one in A->B direction from D */
+		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
+		res = d + ab_norm * dx_len;
+		return true;
+
+	} else {
+		/* have no roots, return D */
+		res = d;
+		return false;
+	}
+}
+
+void
+MulticopterPositionControl::control_auto(float dt)
+{
+	if (!_mode_auto) {
+		_mode_auto = true;
+		/* reset position setpoint on AUTO mode activation */
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+
+	if (_pos_sp_triplet->data().current.valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
+		/* project setpoint to local frame */
+		math::Vector<3> curr_sp;
+		map_projection_project(&_ref_pos,
+				       _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon,
+				       &curr_sp.data[0], &curr_sp.data[1]);
+		curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt);
+
+		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
+		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
+
+		/* convert current setpoint to scaled space */
+		math::Vector<3> curr_sp_s = curr_sp.emult(scale);
+
+		/* by default use current setpoint as is */
+		math::Vector<3> pos_sp_s = curr_sp_s;
+
+		if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) {
+			/* follow "previous - current" line */
+			math::Vector<3> prev_sp;
+			map_projection_project(&_ref_pos,
+						   _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon,
+						   &prev_sp.data[0], &prev_sp.data[1]);
+			prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt);
+
+			if ((curr_sp - prev_sp).length() > MIN_DIST) {
+
+				/* find X - cross point of L1 sphere and trajectory */
+				math::Vector<3> pos_s = _pos.emult(scale);
+				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
+				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
+				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
+				float curr_pos_s_len = curr_pos_s.length();
+				if (curr_pos_s_len < 1.0f) {
+					/* copter is closer to waypoint than L1 radius */
+					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
+					if (_pos_sp_triplet->data().next.valid) {
+						math::Vector<3> next_sp;
+						map_projection_project(&_ref_pos,
+									   _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon,
+									   &next_sp.data[0], &next_sp.data[1]);
+						next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt);
+
+						if ((next_sp - curr_sp).length() > MIN_DIST) {
+							math::Vector<3> next_sp_s = next_sp.emult(scale);
+
+							/* calculate angle prev - curr - next */
+							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
+							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
+
+							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
+							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
+
+							/* cos(b), b = angle pos - curr_sp - prev_sp */
+							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
+
+							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
+								float curr_next_s_len = curr_next_s.length();
+								/* if curr - next distance is larger than L1 radius, limit it */
+								if (curr_next_s_len > 1.0f) {
+									cos_a_curr_next /= curr_next_s_len;
+								}
+
+								/* feed forward position setpoint offset */
+								math::Vector<3> pos_ff = prev_curr_s_norm *
+										cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
+										(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
+								pos_sp_s += pos_ff;
+							}
+						}
+					}
+
+				} else {
+					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
+					if (near) {
+						/* L1 sphere crosses trajectory */
+
+					} else {
+						/* copter is too far from trajectory */
+						/* if copter is behind prev waypoint, go directly to prev waypoint */
+						if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
+							pos_sp_s = prev_sp_s;
+						}
+
+						/* if copter is in front of curr waypoint, go directly to curr waypoint */
+						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
+							pos_sp_s = curr_sp_s;
+						}
+
+						pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
+					}
+				}
+			}
+		}
+
+		/* move setpoint not faster than max allowed speed */
+		math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
+
+		/* difference between current and desired position setpoints, 1 = max speed */
+		math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
+		float d_pos_m_len = d_pos_m.length();
+		if (d_pos_m_len > dt) {
+			pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
+		}
+
+		/* scale result back to normal space */
+		_pos_sp = pos_sp_s.edivide(scale);
+
+		/* update yaw setpoint if needed */
+		if (isfinite(_pos_sp_triplet->data().current.yaw)) {
+			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
+		}
+
+	} else {
+		/* no waypoint, do nothing, setpoint was already reset */
+	}
+}
+
+void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg)
+{
+	parameters_update();
+}
+
+void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
+{
+	static bool reset_int_z = true;
+	static bool reset_int_z_manual = false;
+	static bool reset_int_xy = true;
+	static bool was_armed = false;
+	static uint64_t t_prev = 0;
+
+	uint64_t t = get_time_micros();
+	float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
+	t_prev = t;
+
+	if (_control_mode->data().flag_armed && !was_armed) {
+		/* reset setpoints and integrals on arming */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+		reset_int_z = true;
+		reset_int_xy = true;
+	}
+
+	was_armed = _control_mode->data().flag_armed;
+
+	update_ref();
+
+	if (_control_mode->data().flag_control_altitude_enabled ||
+		_control_mode->data().flag_control_position_enabled ||
+		_control_mode->data().flag_control_climb_rate_enabled ||
+		_control_mode->data().flag_control_velocity_enabled) {
+
+		_pos(0) = _local_pos->data().x;
+		_pos(1) = _local_pos->data().y;
+		_pos(2) = _local_pos->data().z;
+
+		_vel(0) = _local_pos->data().vx;
+		_vel(1) = _local_pos->data().vy;
+		_vel(2) = _local_pos->data().vz;
+
+		_vel_ff.zero();
+		_sp_move_rate.zero();
+
+		/* select control source */
+		if (_control_mode->data().flag_control_manual_enabled) {
+			/* manual control */
+			control_manual(dt);
+			_mode_auto = false;
+
+		} else if (_control_mode->data().flag_control_offboard_enabled) {
+			/* offboard control */
+			control_offboard(dt);
+			_mode_auto = false;
+
+		} else {
+			/* AUTO */
+			control_auto(dt);
+		}
+
+		/* fill local position setpoint */
+		_local_pos_sp_msg.data().timestamp = get_time_micros();
+		_local_pos_sp_msg.data().x = _pos_sp(0);
+		_local_pos_sp_msg.data().y = _pos_sp(1);
+		_local_pos_sp_msg.data().z = _pos_sp(2);
+		_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
+
+		/* publish local position setpoint */
+		if (_local_pos_sp_pub != nullptr) {
+			_local_pos_sp_pub->publish(_local_pos_sp_msg);
+
+		} else {
+			_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
+		}
+
+
+		if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
+			/* idle state, don't run controller and set zero thrust */
+			_R.identity();
+			memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+			_att_sp_msg.data().R_valid = true;
+
+			_att_sp_msg.data().roll_body = 0.0f;
+			_att_sp_msg.data().pitch_body = 0.0f;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
+			_att_sp_msg.data().thrust = 0.0f;
+
+			_att_sp_msg.data().timestamp = get_time_micros();
+
+			/* publish attitude setpoint */
+			if (_att_sp_pub != nullptr) {
+				_att_sp_pub->publish(_att_sp_msg);
+
+			} else {
+				_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+			}
+
+		} else {
+			/* run position & altitude controllers, calculate velocity setpoint */
+			math::Vector<3> pos_err = _pos_sp - _pos;
+
+			_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
+
+			if (!_control_mode->data().flag_control_altitude_enabled) {
+				_reset_alt_sp = true;
+				_vel_sp(2) = 0.0f;
+			}
+
+			if (!_control_mode->data().flag_control_position_enabled) {
+				_reset_pos_sp = true;
+				_vel_sp(0) = 0.0f;
+				_vel_sp(1) = 0.0f;
+			}
+
+			/* use constant descend rate when landing, ignore altitude setpoint */
+			if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
+				_vel_sp(2) = _params.land_speed;
+			}
+
+			_global_vel_sp_msg.data().vx = _vel_sp(0);
+			_global_vel_sp_msg.data().vy = _vel_sp(1);
+			_global_vel_sp_msg.data().vz = _vel_sp(2);
+
+			/* publish velocity setpoint */
+			if (_global_vel_sp_pub != nullptr) {
+				_global_vel_sp_pub->publish(_global_vel_sp_msg);
+
+			} else {
+				_global_vel_sp_pub = _n.advertise<px4_vehicle_global_velocity_setpoint>();
+			}
+
+			if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) {
+				/* reset integrals if needed */
+				if (_control_mode->data().flag_control_climb_rate_enabled) {
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = _params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = _manual_control_sp->data().z;
+
+							if (i < _params.thr_min) {
+								i = _params.thr_min;
+
+							} else if (i > _params.thr_max) {
+								i = _params.thr_max;
+							}
+						}
+
+						_thrust_int(2) = -i;
+					}
+
+				} else {
+					reset_int_z = true;
+				}
+
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						_thrust_int(0) = 0.0f;
+						_thrust_int(1) = 0.0f;
+					}
+
+				} else {
+					reset_int_xy = true;
+				}
+
+				/* velocity error */
+				math::Vector<3> vel_err = _vel_sp - _vel;
+
+				/* derivative of velocity error, not includes setpoint acceleration */
+				math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+				_vel_prev = _vel;
+
+				/* thrust vector in NED frame */
+				math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int;
+
+				if (!_control_mode->data().flag_control_velocity_enabled) {
+					thrust_sp(0) = 0.0f;
+					thrust_sp(1) = 0.0f;
+				}
+
+				if (!_control_mode->data().flag_control_climb_rate_enabled) {
+					thrust_sp(2) = 0.0f;
+				}
+
+				/* limit thrust vector and check for saturation */
+				bool saturation_xy = false;
+				bool saturation_z = false;
+
+				/* limit min lift */
+				float thr_min = _params.thr_min;
+
+				if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) {
+					/* don't allow downside thrust direction in manual attitude mode */
+					thr_min = 0.0f;
+				}
+
+				float tilt_max = _params.tilt_max_air;
+
+				/* adjust limits for landing mode */
+				if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid &&
+					_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
+					/* limit max tilt and min lift when landing */
+					tilt_max = _params.tilt_max_land;
+
+					if (thr_min < 0.0f) {
+						thr_min = 0.0f;
+					}
+				}
+
+				/* limit min lift */
+				if (-thrust_sp(2) < thr_min) {
+					thrust_sp(2) = -thr_min;
+					saturation_z = true;
+				}
+
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					/* limit max tilt */
+					if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
+						/* absolute horizontal thrust */
+						float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+
+						if (thrust_sp_xy_len > 0.01f) {
+							/* max horizontal thrust for given vertical thrust*/
+							float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
+
+							if (thrust_sp_xy_len > thrust_xy_max) {
+								float k = thrust_xy_max / thrust_sp_xy_len;
+								thrust_sp(0) *= k;
+								thrust_sp(1) *= k;
+								saturation_xy = true;
+							}
+						}
+					}
+
+				} else {
+					/* thrust compensation for altitude only control mode */
+					float att_comp;
+
+					if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
+						att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
+
+					} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
+						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f;
+						saturation_z = true;
+
+					} else {
+						att_comp = 1.0f;
+						saturation_z = true;
+					}
+
+					thrust_sp(2) *= att_comp;
+				}
+
+				/* limit max thrust */
+				float thrust_abs = thrust_sp.length();
+
+				if (thrust_abs > _params.thr_max) {
+					if (thrust_sp(2) < 0.0f) {
+						if (-thrust_sp(2) > _params.thr_max) {
+							/* thrust Z component is too large, limit it */
+							thrust_sp(0) = 0.0f;
+							thrust_sp(1) = 0.0f;
+							thrust_sp(2) = -_params.thr_max;
+							saturation_xy = true;
+							saturation_z = true;
+
+						} else {
+							/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
+							float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
+							float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+							float k = thrust_xy_max / thrust_xy_abs;
+							thrust_sp(0) *= k;
+							thrust_sp(1) *= k;
+							saturation_xy = true;
+						}
+
+					} else {
+						/* Z component is negative, going down, simply limit thrust vector */
+						float k = _params.thr_max / thrust_abs;
+						thrust_sp *= k;
+						saturation_xy = true;
+						saturation_z = true;
+					}
+
+					thrust_abs = _params.thr_max;
+				}
+
+				/* update integrals */
+				if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) {
+					_thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
+					_thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
+				}
+
+				if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) {
+					_thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
+
+					/* protection against flipping on ground when landing */
+					if (_thrust_int(2) > 0.0f) {
+						_thrust_int(2) = 0.0f;
+					}
+				}
+
+				/* calculate attitude setpoint from thrust vector */
+				if (_control_mode->data().flag_control_velocity_enabled) {
+					/* desired body_z axis = -normalize(thrust_vector) */
+					math::Vector<3> body_x;
+					math::Vector<3> body_y;
+					math::Vector<3> body_z;
+
+					if (thrust_abs > SIGMA) {
+						body_z = -thrust_sp / thrust_abs;
+
+					} else {
+						/* no thrust, set Z axis to safe value */
+						body_z.zero();
+						body_z(2) = 1.0f;
+					}
+
+					/* vector of desired yaw direction in XY plane, rotated by PI/2 */
+					math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f);
+
+					if (fabsf(body_z(2)) > SIGMA) {
+						/* desired body_x axis, orthogonal to body_z */
+						body_x = y_C % body_z;
+
+						/* keep nose to front while inverted upside down */
+						if (body_z(2) < 0.0f) {
+							body_x = -body_x;
+						}
+
+						body_x.normalize();
+
+					} else {
+						/* desired thrust is in XY plane, set X downside to construct correct matrix,
+							* but yaw component will not be used actually */
+						body_x.zero();
+						body_x(2) = 1.0f;
+					}
+
+					/* desired body_y axis */
+					body_y = body_z % body_x;
+
+					/* fill rotation matrix */
+					for (int i = 0; i < 3; i++) {
+						_R(i, 0) = body_x(i);
+						_R(i, 1) = body_y(i);
+						_R(i, 2) = body_z(i);
+					}
+
+					/* copy rotation matrix to attitude setpoint topic */
+					memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+					_att_sp_msg.data().R_valid = true;
+
+					/* calculate euler angles, for logging only, must not be used for control */
+					math::Vector<3> euler = _R.to_euler();
+					_att_sp_msg.data().roll_body = euler(0);
+					_att_sp_msg.data().pitch_body = euler(1);
+					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+				} else if (!_control_mode->data().flag_control_manual_enabled) {
+					/* autonomous altitude control without position control (failsafe landing),
+						* force level attitude, don't change yaw */
+					_R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
+
+					/* copy rotation matrix to attitude setpoint topic */
+					memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
+					_att_sp_msg.data().R_valid = true;
+
+					_att_sp_msg.data().roll_body = 0.0f;
+					_att_sp_msg.data().pitch_body = 0.0f;
+				}
+
+				_att_sp_msg.data().thrust = thrust_abs;
+
+				_att_sp_msg.data().timestamp = get_time_micros();
+
+				/* publish attitude setpoint */
+				if (_att_sp_pub != nullptr) {
+					_att_sp_pub->publish(_att_sp_msg);
+
+				} else {
+					_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+				}
+
+			} else {
+				reset_int_z = true;
+			}
+		}
+
+	} else {
+		/* position controller disabled, reset setpoints */
+		_reset_alt_sp = true;
+		_reset_pos_sp = true;
+		_mode_auto = false;
+		reset_int_z = true;
+		reset_int_xy = true;
+	}
+
+	/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+	reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
new file mode 100644
index 0000000000..05bd1387be
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control.h
+ * Multicopter position controller.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <px4.h>
+#include <cstdio>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+// #include <poll.h>
+// #include <drivers/drv_hrt.h>
+// #include <arch/board/board.h>
+// #include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+// #include <mavlink/mavlink_log.h>
+
+using namespace px4;
+
+class MulticopterPositionControl
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterPositionControl();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~MulticopterPositionControl();
+
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+	void handle_parameter_update(const px4_parameter_update &msg);
+
+	void spin() { _n.spin(); }
+
+protected:
+	const float alt_ctl_dz = 0.2f;
+
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
+
+	Publisher<px4_vehicle_attitude_setpoint>	*_att_sp_pub;			/**< attitude setpoint publication */
+	Publisher<px4_vehicle_local_position_setpoint>	*_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
+	Publisher<px4_vehicle_global_velocity_setpoint>		*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+
+
+	Subscriber<px4_vehicle_attitude> *_att;			    /**< vehicle attitude */
+	Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
+	Subscriber<px4_vehicle_control_mode> *_control_mode;	    /**< vehicle control mode */
+	Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
+	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
+	Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
+	Subscriber<px4_vehicle_local_position> *_local_pos;			    /**< local position */
+	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;			    /**< local position */
+	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;			    /**< local position */
+	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;			    /**< local position */
+
+	px4_vehicle_attitude_setpoint _att_sp_msg;
+	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
+	px4_vehicle_global_velocity_setpoint _global_vel_sp_msg;
+
+	px4::NodeHandle _n;
+
+
+	struct {
+		px4_param_t thr_min;
+		px4_param_t thr_max;
+		px4_param_t z_p;
+		px4_param_t z_vel_p;
+		px4_param_t z_vel_i;
+		px4_param_t z_vel_d;
+		px4_param_t z_vel_max;
+		px4_param_t z_ff;
+		px4_param_t xy_p;
+		px4_param_t xy_vel_p;
+		px4_param_t xy_vel_i;
+		px4_param_t xy_vel_d;
+		px4_param_t xy_vel_max;
+		px4_param_t xy_ff;
+		px4_param_t tilt_max_air;
+		px4_param_t land_speed;
+		px4_param_t tilt_max_land;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	struct {
+		float thr_min;
+		float thr_max;
+		float tilt_max_air;
+		float land_speed;
+		float tilt_max_land;
+
+		math::Vector<3> pos_p;
+		math::Vector<3> vel_p;
+		math::Vector<3> vel_i;
+		math::Vector<3> vel_d;
+		math::Vector<3> vel_ff;
+		math::Vector<3> vel_max;
+		math::Vector<3> sp_offs_max;
+	}		_params;
+
+	struct map_projection_reference_s _ref_pos;
+	float _ref_alt;
+	uint64_t _ref_timestamp;
+
+	bool _reset_pos_sp;
+	bool _reset_alt_sp;
+	bool _mode_auto;
+
+	math::Vector<3> _pos;
+	math::Vector<3> _pos_sp;
+	math::Vector<3> _vel;
+	math::Vector<3> _vel_sp;
+	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
+
+	math::Vector<3> _thrust_int;
+	math::Matrix<3, 3> _R;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int			parameters_update();
+
+	/**
+	 * Update control outputs
+	 */
+	void		control_update();
+
+	static float	scale_control(float ctl, float end, float dz);
+
+	/**
+	 * Update reference for local position projection
+	 */
+	void		update_ref();
+	/**
+	 * Reset position setpoint to current position
+	 */
+	void		reset_pos_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
+	bool		cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+					const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
+
+	/**
+	 * Set position setpoint for AUTO
+	 */
+	void		control_auto(float dt);
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+};
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
new file mode 100644
index 0000000000..0b5775736e
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_main.cpp
+ * Multicopter position controller.
+ *
+ * The controller has two loops: P loop for position error and PID loop for velocity error.
+ * Output of velocity controller is thrust vector that splitted to thrust direction
+ * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mc_pos_control.h"
+
+bool thread_running = false;     /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+	px4::init(argc, argv, "mc_pos_control_m");
+
+	PX4_INFO("starting");
+	MulticopterPositionControl posctl;
+	thread_running = true;
+	posctl.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
new file mode 100644
index 0000000000..c741a7f0a6
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
@@ -0,0 +1,212 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author 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) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_params.c
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_pos_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Minimum thrust
+ *
+ * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
+
+/**
+ * Maximum thrust
+ *
+ * Limit max allowed thrust.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
+
+/**
+ * Proportional gain for vertical position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
+
+/**
+ * Proportional gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
+
+/**
+ * Integral gain for vertical velocity error
+ *
+ * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
+
+/**
+ * Differential gain for vertical velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
+
+/**
+ * Maximum vertical velocity
+ *
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
+
+/**
+ * Vertical velocity feed forward
+ *
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
+
+/**
+ * Proportional gain for horizontal position error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
+
+/**
+ * Proportional gain for horizontal velocity error
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
+
+/**
+ * Integral gain for horizontal velocity error
+ *
+ * Non-zero value allows to resist wind.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
+
+/**
+ * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
+
+/**
+ * Maximum horizontal velocity
+ *
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
+
+/**
+ * Horizontal velocity feed forward
+ *
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
+
+/**
+ * Maximum tilt angle in air
+ *
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
+
+/**
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
+
+/**
+ * Landing descend rate
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
+
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
new file mode 100644
index 0000000000..fec3bcb7cc
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author 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 mc_pos_control_params.h
+ * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPC_Z_P_DEFAULT 1.0f
+#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_Z_FF_DEFAULT 0.5f
+#define PARAM_MPC_XY_P_DEFAULT 1.0f
+#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPC_XY_FF_DEFAULT 0.5f
+#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
+
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
new file mode 100644
index 0000000000..87996d93ba
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 - 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_pos_control_m_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
+int mc_pos_control_m_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: mc_pos_control_m {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_pos_control_m",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       3000,
+				       main,
+					(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk
new file mode 100644
index 0000000000..2852ebbec8
--- /dev/null
+++ b/src/modules/mc_pos_control_multiplatform/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build multicopter position controller
+#
+
+MODULE_COMMAND	= mc_pos_control_m
+
+SRCS		= mc_pos_control_main.cpp \
+		  mc_pos_control_start_nuttx.cpp \
+		  mc_pos_control.cpp \
+		  mc_pos_control_params.c
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index 1bd4509caf..f8561fa3b7 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -62,6 +62,10 @@
 #include <px4_actuator_armed.h>
 #include <px4_parameter_update.h>
 #include <px4_vehicle_status.h>
+#include <px4_vehicle_local_position_setpoint.h>
+#include <px4_vehicle_global_velocity_setpoint.h>
+#include <px4_vehicle_local_position.h>
+#include <px4_position_setpoint_triplet.h>
 #endif
 
 #else
@@ -85,6 +89,10 @@
 #include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
 #include <platforms/nuttx/px4_messages/px4_parameter_update.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h>
+#include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>

From 8e7974e2e29a75daf5a55e723f6d6a4b416c252f Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 08:25:42 +0100
Subject: [PATCH 391/416] fix uorb constants for test functions

---
 .../state_machine_helper_test.cpp             | 346 +++++++++---------
 1 file changed, 173 insertions(+), 173 deletions(-)

diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 3cfa8b4c6c..4ddb4e0fbf 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         bool            armed;          // actuator_armed_s.armed
         bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
     } ArmingTransitionVolatileState_t;
-    
+
     // This structure represents a test case for arming_state_transition. It contains the machine
     // state prior to transtion, the requested state to transition to and finally the expected
     // machine state after transition.
@@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
         transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
     } ArmingTransitionTest_t;
-    
+
     // We use these defines so that our test cases are more readable
     #define ATT_ARMED true
     #define ATT_DISARMED false
@@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
     #define ATT_SAFETY_NOT_AVAILABLE true
     #define ATT_SAFETY_OFF true
     #define ATT_SAFETY_ON false
-    
+
     // These are test cases for arming_state_transition
     static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
         // TRANSITION_NOT_CHANGED tests
-        
+
         { "no transition: identical states",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+
         // TRANSITION_CHANGED tests
-        
+
         // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
-        
+
         { "transition: init to standby",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: init to standby error",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: init to reboot",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to init",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to standby error",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to reboot",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed to standby",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed to armed error",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED_ERROR,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED_ERROR,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: armed error to standby error",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY_ERROR,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby error to reboot",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: in air restore to armed",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: in air restore to reboot",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // hil on tests, standby error to standby not normally allowed
-        
+
         { "transition: standby error to standby, hil on",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // Safety switch arming tests
-        
+
         { "transition: standby to armed, no safety switch",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         { "transition: standby to armed, safety switch off",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // standby error
         { "transition: armed error to standby error requested standby",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+
         // TRANSITION_DENIED tests
-        
+
         // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
-        
+
         { "no transition: init to armed",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby to armed error",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED_ERROR,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED_ERROR,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed to init",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_INIT,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_INIT,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed to reboot",
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed error to armed",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: armed error to reboot",
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_REBOOT,
-            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_REBOOT,
+            { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby error to armed",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: standby error to standby",
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: reboot to armed",
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         { "no transition: in air restore to standby",
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         // Sensor tests
-        
+
         { "no transition: init to standby - sensors not initialized",
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_STANDBY,
-            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-        
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_STANDBY,
+            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+
         // Safety switch arming tests
-        
+
         { "no transition: init to standby, safety switch on",
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            ARMING_STATE_ARMED,
-            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            vehicle_status_s::ARMING_STATE_ARMED,
+            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
     };
-    
+
 	struct vehicle_status_s status;
 	struct safety_s         safety;
 	struct actuator_armed_s armed;
-    
+
     size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
     for (size_t i=0; i<cArmingTransitionTests; i++) {
         const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
-        
+
         // Setup initial machine state
         status.arming_state = test->current_state.arming_state;
         status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
@@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;
         armed.ready_to_arm = test->current_state.ready_to_arm;
-        
+
         // Attempt transition
         transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
-        
+
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
         ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
@@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		main_state_t	to_state;				// State to transition to
 		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
 	} MainTransitionTest_t;
-	
+
 	// Bits for condition_bits
 	#define MTT_ALL_NOT_VALID		0
 	#define MTT_ROTARY_WING			1 << 0
@@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 	#define MTT_LOC_POS_VALID		1 << 2
 	#define MTT_HOME_POS_VALID		1 << 3
 	#define MTT_GLOBAL_POS_VALID		1 << 4
-	
+
 	static const MainTransitionTest_t rgMainTransitionTests[] = {
-		
+
 		// TRANSITION_NOT_CHANGED tests
-		
+
 		{ "no transition: identical states",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
-				
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+
 		// TRANSITION_CHANGED tests
-		
+
 		{ "transition: MANUAL to ACRO",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
 		{ "transition: ACRO to MANUAL",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_LOITER to MANUAL - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - not rotary",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
 			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
 		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
 			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
 		{ "transition: ALTCTL to MANUAL - local altitude valid",
 			MTT_LOC_ALT_VALID,
-			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
 			MTT_LOC_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
 			MTT_LOC_POS_VALID,
-			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		// TRANSITION_DENIED tests
 
 		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
 			MTT_HOME_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
 			MTT_GLOBAL_POS_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
 			MTT_ROTARY_WING,
-			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
-		
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
 			MTT_ALL_NOT_VALID,
-			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
 	};
-	
+
 	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
 	for (size_t i=0; i<cMainTransitionTests; i++) {
 		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
@@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
 		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
 		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
-		
+
 		// Attempt transition
 		transition_result_t result = main_state_transition(&current_state, test->to_state);
-		
+
 		// Validate result of transition
 		ut_assert(test->assertMsg, test->expected_transition_result == result);
 		if (test->expected_transition_result == result) {
@@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void)
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
-	
+
 	return (_tests_failed == 0);
 }
 
-ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

From 66007d56ef35ebc1f11ac83f2347bfc22b9664f9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 09:08:58 +0100
Subject: [PATCH 392/416] fix uorb constants in uavcan module

---
 src/modules/uavcan/uavcan_main.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 4dc03b61b7..b93a95f965 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -381,7 +381,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
 						controls_updated = true;
@@ -393,14 +393,14 @@ int UavcanNode::run()
 			/*
 			  see if we have any direct actuator updates
 			 */
-			if (_actuator_direct_sub != -1 && 
+			if (_actuator_direct_sub != -1 &&
 			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
 			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
 			    !_test_in_progress) {
 				if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
 					_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
 				}
-				memcpy(&_outputs.output[0], &_actuator_direct.values[0], 
+				memcpy(&_outputs.output[0], &_actuator_direct.values[0],
 				       _actuator_direct.nvalues*sizeof(float));
 				_outputs.noutputs = _actuator_direct.nvalues;
 				new_output = true;
@@ -476,7 +476,7 @@ int UavcanNode::run()
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			// Update the armed status and check that we're not locked down and motor 
+			// Update the armed status and check that we're not locked down and motor
 			// test is not running
 			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
 
@@ -502,7 +502,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
 int
 UavcanNode::teardown()
 {
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
@@ -526,7 +526,7 @@ UavcanNode::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	// the first fd used by CAN
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);

From a6ca4c2796a825ac1878c1d570afc0afc4fed74d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 11:17:23 +0100
Subject: [PATCH 393/416] initialize all subscribers

---
 src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index 2685a99c32..aa23e0a60d 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -78,6 +78,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 			&MulticopterPositionControl::handle_parameter_update, this, 1000);
 	_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
 	_armed = _n.subscribe<px4_actuator_armed>(0);
+	_local_pos = _n.subscribe<px4_vehicle_local_position>(0);
+	_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0);
 	_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
 	_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
 

From 9245f28fb84e87505c6024121720bbd0776e64a0 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 28 Jan 2015 17:46:22 +0100
Subject: [PATCH 394/416] mc attctl multiplatform: fix memcpy

---
 .../mc_att_control_multiplatform/mc_att_control_base.cpp    | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index fcfee28dc7..5577c5f03e 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -97,7 +97,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		if (_v_control_mode->data().flag_control_velocity_enabled
 		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
+			//XXX get rid of memcpy
+			memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
 		}
 
 		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
@@ -156,7 +157,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
+		//XXX get rid of memcpy
+		memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;

From 8a6b94adbfa58245d13354216168de86e888c782 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 10:28:56 +0100
Subject: [PATCH 395/416] attitude estimator dummy node: add timestamp

---
 .../ros/nodes/attitude_estimator/attitude_estimator.cpp         | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index bcee0b4799..f744aa11c2 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -43,6 +43,7 @@
 #include <px4/vehicle_attitude.h>
 #include <mathlib/mathlib.h>
 #include <platforms/px4_defines.h>
+#include <platforms/px4_middleware.h>
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
@@ -92,6 +93,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 	// msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y;
 	// msg_v_att.yawspeed = -(float)msg->twist[index].angular.z;
 
+	msg_v_att.timestamp = px4::get_time_micros();
 	_vehicle_attitude_pub.publish(msg_v_att);
 }
 

From 6f4f5d637de51272a2bc9fd3a8db0a2755d763c6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 10:29:15 +0100
Subject: [PATCH 396/416] first version of position estimator dummy node

---
 CMakeLists.txt                                |   9 ++
 .../position_estimator/position_estimator.cpp | 107 ++++++++++++++++++
 .../position_estimator/position_estimator.h   |  62 ++++++++++
 3 files changed, 178 insertions(+)
 create mode 100644 src/platforms/ros/nodes/position_estimator/position_estimator.cpp
 create mode 100644 src/platforms/ros/nodes/position_estimator/position_estimator.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6a368d78fc..0c81607b00 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -204,6 +204,15 @@ target_link_libraries(attitude_estimator
   px4
 )
 
+## Position Estimator dummy
+add_executable(position_estimator
+	src/platforms/ros/nodes/position_estimator/position_estimator.cpp)
+add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(position_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
 ## Manual input
 add_executable(manual_input
 	src/platforms/ros/nodes/manual_input/manual_input.cpp)
diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
new file mode 100644
index 0000000000..231e9763a8
--- /dev/null
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+
+#include "position_estimator.h"
+
+#include <px4/vehicle_local_position.h>
+#include <mathlib/mathlib.h>
+#include <platforms/px4_defines.h>
+#include <platforms/px4_middleware.h>
+#include <vector>
+#include <string>
+#include <gazebo_msgs/ModelStates.h>
+
+PositionEstimator::PositionEstimator() :
+	_n(),
+	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::model_states_callback, this)),
+	_vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)),
+	_startup_time(px4::get_time_micros())
+{
+}
+
+void PositionEstimator::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg)
+{
+	//XXX: use a proper sensor topic
+
+	px4::vehicle_local_position msg_v_l_pos;
+
+	/* Fill px4 position topic with contents from modelstates topic */
+	int index = 0;
+	//XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when
+	//gazebo rearranges indexes.
+	for(std::vector<std::string>::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) {
+		if (*it == "iris" || *it == "ardrone") {
+			index = it -  msg->name.begin();
+			break;
+		}
+	}
+	msg_v_l_pos.xy_valid = true;
+	msg_v_l_pos.z_valid = true;
+	msg_v_l_pos.v_xy_valid = true;
+	msg_v_l_pos.v_z_valid = true;
+
+	msg_v_l_pos.x = msg->pose[index].position.x;
+	msg_v_l_pos.y = -msg->pose[index].position.y;
+	msg_v_l_pos.z = -msg->pose[index].position.z;
+	msg_v_l_pos.vx = msg->twist[index].linear.x;
+	msg_v_l_pos.vy = -msg->twist[index].linear.y;
+	msg_v_l_pos.vz = -msg->twist[index].linear.z;
+
+	msg_v_l_pos.xy_global = true;
+	msg_v_l_pos.z_global = true;
+	msg_v_l_pos.ref_timestamp = _startup_time;
+	msg_v_l_pos.ref_lat = 47.378301;
+	msg_v_l_pos.ref_lon = 8.538777;
+	msg_v_l_pos.ref_alt = 1200.0f;
+
+
+	msg_v_l_pos.timestamp = px4::get_time_micros();
+	_vehicle_position_pub.publish(msg_v_l_pos);
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "position_estimator");
+	PositionEstimator m;
+
+	ros::spin();
+
+	return 0;
+}
diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h
new file mode 100644
index 0000000000..40c4baa235
--- /dev/null
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator.h
+ * Dummy position estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <gazebo_msgs/ModelStates.h>
+#include <sensor_msgs/Imu.h>
+
+class PositionEstimator
+{
+public:
+	PositionEstimator();
+
+	~PositionEstimator() {}
+
+protected:
+	void model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg);
+
+	ros::NodeHandle _n;
+	ros::Subscriber _sub_modelstates;
+	ros::Publisher _vehicle_position_pub;
+
+	uint64_t _startup_time;
+
+
+};

From 8b62e003f0edbd8fe82286eaead83f0b3ef88a8c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 10:30:07 +0100
Subject: [PATCH 397/416] add dummy position estimator to launch file

---
 launch/multicopter.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 96ff3ad99c..5d1a351262 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -6,6 +6,7 @@
 	<node pkg="px4" name="commander" type="commander"/>
 	<node pkg="px4" name="mc_mixer" type="mc_mixer"/>
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
+	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 </group>
 

From 9a5ea31784bacb57b9a86134fb612119609fc08c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 10:33:12 +0100
Subject: [PATCH 398/416] add pos controller to launch file

---
 launch/multicopter.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 5d1a351262..0f8cc5132d 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -8,6 +8,7 @@
 	<node pkg="px4" name="attitude_estimator" type="attitude_estimator"/>
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
+	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
 </group>
 
 </launch>

From 5b5a4b73864bb3331830d232fc3bb1bf7372d844 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 13:36:53 +0100
Subject: [PATCH 399/416] function rename

---
 .../ros/nodes/position_estimator/position_estimator.cpp       | 4 ++--
 .../ros/nodes/position_estimator/position_estimator.h         | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
index 231e9763a8..fae304bf49 100644
--- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
@@ -50,13 +50,13 @@
 
 PositionEstimator::PositionEstimator() :
 	_n(),
-	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::model_states_callback, this)),
+	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)),
 	_vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)),
 	_startup_time(px4::get_time_micros())
 {
 }
 
-void PositionEstimator::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg)
+void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
 {
 	//XXX: use a proper sensor topic
 
diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h
index 40c4baa235..4d396f39a9 100644
--- a/src/platforms/ros/nodes/position_estimator/position_estimator.h
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h
@@ -50,7 +50,7 @@ public:
 	~PositionEstimator() {}
 
 protected:
-	void model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg);
+	void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _sub_modelstates;

From 3c79b2a586286615ef00e1584c7c2f74887e38cd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 13:37:37 +0100
Subject: [PATCH 400/416] manual input dummy node: extend to support switching
 between modes

also fixing thrust input
---
 .../ros/nodes/manual_input/manual_input.cpp   | 80 ++++++++++++++-----
 .../ros/nodes/manual_input/manual_input.h     | 23 +++++-
 2 files changed, 82 insertions(+), 21 deletions(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 688df50e0c..7e1ae7a174 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -40,13 +40,14 @@
 
 #include "manual_input.h"
 
-#include <px4/manual_control_setpoint.h>
 #include <platforms/px4_middleware.h>
 
 ManualInput::ManualInput() :
 	_n(),
 	_joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
-	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1))
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)),
+	_buttons_state(),
+	_msg_mc_sp()
 {
 	double dz_default = 0.2;
 	/* Load parameters, default values work for Microsoft XBox Controller */
@@ -63,40 +64,46 @@ ManualInput::ManualInput() :
 	_n.param("map_z", _param_axes_map[2], 2);
 	_n.param("scale_z", _param_axes_scale[2], -0.5);
 	_n.param("off_z", _param_axes_offset[2], -1.0);
-	_n.param("dz_z", _param_axes_dz[2], dz_default);
+	_n.param("dz_z", _param_axes_dz[2], 0.0);
 
 	_n.param("map_r", _param_axes_map[3], 0);
 	_n.param("scale_r", _param_axes_scale[3], -1.0);
 	_n.param("off_r", _param_axes_offset[3], 0.0);
 	_n.param("dz_r", _param_axes_dz[3], dz_default);
 
+	_n.param("map_manual", _param_buttons_map[0], 0);
+	_n.param("map_altctl", _param_buttons_map[1], 1);
+	_n.param("map_posctl", _param_buttons_map[2], 2);
+	_n.param("map_auto_mission", _param_buttons_map[3], 3);
+	_n.param("map_auto_loiter", _param_buttons_map[4], 4);
+	_n.param("map_auto_rtl", _param_buttons_map[5], 4);
+
+	/* Default to manual */
+	_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+
 }
 
 void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
 {
-	px4::manual_control_setpoint msg_out;
 
 	/* Fill px4 manual control setpoint topic with contents from ros joystick */
 	/* Map sticks to x, y, z, r */
-	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x);
-	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y);
-	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z);
-	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r);
-	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
+	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x);
+	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y);
+	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z);
+	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r);
+	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r);
 
 	/* Map buttons to switches */
-	//XXX todo
-	/* for now just publish switches in position for manual flight */
-	msg_out.mode_switch = 0;
-	msg_out.return_switch = 0;
-	msg_out.posctl_switch = 0;
-	msg_out.loiter_switch = 0;
-	msg_out.acro_switch = 0;
-	msg_out.offboard_switch = 0;
+	MapButtons(msg, _msg_mc_sp);
 
-	msg_out.timestamp = px4::get_time_micros();
+	_msg_mc_sp.timestamp = px4::get_time_micros();
 
-	_man_ctrl_sp_pub.publish(msg_out);
+	_man_ctrl_sp_pub.publish(_msg_mc_sp);
 }
 
 void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset,
@@ -106,6 +113,41 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do
 
 	if (val + offset > deadzone || val + offset < -deadzone) {
 		out = (float)((val + offset)) * scale;
+	} else {
+		out = 0.0f;
+	}
+}
+
+void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) {
+	msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
+	msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
+
+	if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+
+	} else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+
+	} else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+
+	} else {
+		ROS_WARN("requested mode via joystick that is not implemented");
 	}
 }
 
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index 93e0abe64d..bf704f6757 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -39,6 +39,7 @@
 */
 
 #include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
 #include <sensor_msgs/Joy.h>
 
 class ManualInput
@@ -55,20 +56,38 @@ protected:
 	void JoyCallback(const sensor_msgs::JoyConstPtr &msg);
 
 	/**
-	 * Helper function to map and scale joystick input
+	 * Helper function to map and scale joystick axis
 	 */
 	void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
 		     float &out);
+	/**
+	 * Helper function to map joystick buttons
+	 */
+	void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _joy_sub;
 	ros::Publisher _man_ctrl_sp_pub;
 
 	/* Parameters */
+	enum MAIN_STATE {
+		MAIN_STATE_MANUAL = 0,
+		MAIN_STATE_ALTCTL,
+		MAIN_STATE_POSCTL,
+		MAIN_STATE_AUTO_MISSION,
+		MAIN_STATE_AUTO_LOITER,
+		MAIN_STATE_AUTO_RTL,
+		MAIN_STATE_MAX
+	};
+
+	int _param_buttons_map[MAIN_STATE_MAX];	    /**< joystick button mapping, order according to MAIN_STATE */
+	bool _buttons_state[MAIN_STATE_MAX];	    /**< joystick button state of last iteration,
+						      order according to MAIN_STATE */
+
 	int _param_axes_map[4];
 	double _param_axes_scale[4];
 	double _param_axes_offset[4];
 	double _param_axes_dz[4];
 
-
+	px4::manual_control_setpoint _msg_mc_sp;
 };

From 5237364a5a6a5e765bbb30cb20eb399dcd14489a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 13:38:39 +0100
Subject: [PATCH 401/416] commander dummy node: extend to support switching
 between modes

---
 .../ros/nodes/commander/commander.cpp         | 64 +++++++++++++++----
 src/platforms/ros/nodes/commander/commander.h |  9 +++
 2 files changed, 61 insertions(+), 12 deletions(-)

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index b9fc296f9d..fee32b55f9 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -40,9 +40,6 @@
 
 #include "commander.h"
 
-#include <px4/manual_control_setpoint.h>
-#include <px4/vehicle_control_mode.h>
-#include <px4/vehicle_status.h>
 #include <platforms/px4_middleware.h>
 
 Commander::Commander() :
@@ -62,12 +59,9 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	px4::vehicle_control_mode msg_vehicle_control_mode;
 	px4::vehicle_status msg_vehicle_status;
 
-	/* fill vehicle control mode */
-	//XXX hardcoded
+	/* fill vehicle control mode based on (faked) stick positions*/
+	EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode);
 	msg_vehicle_control_mode.timestamp = px4::get_time_micros();
-	msg_vehicle_control_mode.flag_control_manual_enabled = true;
-	msg_vehicle_control_mode.flag_control_rates_enabled = true;
-	msg_vehicle_control_mode.flag_control_attitude_enabled = true;
 
 	/* fill actuator armed */
 	float arm_th = 0.95;
@@ -77,21 +71,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 		/* Check for disarm */
 		if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
 			_msg_actuator_armed.armed = false;
+			msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY;
 		}
 
 	} else {
 		/* Check for arm */
 		if (msg->r > arm_th && msg->z < (1 - arm_th)) {
 			_msg_actuator_armed.armed = true;
+			msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
 		}
 	}
 
 	/* fill vehicle status */
-	//XXX hardcoded
 	msg_vehicle_status.timestamp = px4::get_time_micros();
-	msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
-	msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
-	msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
 	msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
 	msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
 	msg_vehicle_status.is_rotary_wing = true;
@@ -107,6 +99,54 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	}
 }
 
+void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
+		px4::vehicle_status &msg_vehicle_status,
+		px4::vehicle_control_mode &msg_vehicle_control_mode) {
+	// XXX this is a minimal implementation. If more advanced functionalities are
+	// needed consider a full port of the commander
+
+	switch (msg->mode_switch) {
+		case px4::manual_control_setpoint::SWITCH_POS_NONE:
+		ROS_WARN("Joystick button mapping error, main mode not set");
+		break;
+
+	case px4::manual_control_setpoint::SWITCH_POS_OFF:		// MANUAL
+		msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
+		msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
+		msg_vehicle_control_mode.flag_control_manual_enabled = true;
+		msg_vehicle_control_mode.flag_control_rates_enabled = true;
+		msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+		msg_vehicle_control_mode.flag_control_altitude_enabled = false;
+		msg_vehicle_control_mode.flag_control_climb_rate_enabled = false;
+		msg_vehicle_control_mode.flag_control_position_enabled = false;
+
+		break;
+
+	case px4::manual_control_setpoint::SWITCH_POS_MIDDLE:		// ASSIST
+		if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) {
+			msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL;
+			msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL;
+			msg_vehicle_control_mode.flag_control_manual_enabled = true;
+			msg_vehicle_control_mode.flag_control_rates_enabled = true;
+			msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_altitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
+			msg_vehicle_control_mode.flag_control_position_enabled = true;
+		} else {
+			msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL;
+			msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL;
+			msg_vehicle_control_mode.flag_control_manual_enabled = true;
+			msg_vehicle_control_mode.flag_control_rates_enabled = true;
+			msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_altitude_enabled = true;
+			msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
+			msg_vehicle_control_mode.flag_control_position_enabled = false;
+		}
+		break;
+	}
+
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "commander");
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index bd4092b3a2..f251f7c1a0 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -40,6 +40,8 @@
 
 #include "ros/ros.h"
 #include <px4/manual_control_setpoint.h>
+#include <px4/vehicle_control_mode.h>
+#include <px4/vehicle_status.h>
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
 
@@ -56,6 +58,13 @@ protected:
 	 */
 	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
+	/**
+	 * Set control mode flags based on stick positions (equiv to code in px4 commander)
+	 */
+	void EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
+			px4::vehicle_status &msg_vehicle_status,
+			px4::vehicle_control_mode &msg_vehicle_control_mode);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
 	ros::Publisher _vehicle_control_mode_pub;

From 6dead1d576b91811ba4bcd988ed4f4151eacafcc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 14:09:25 +0100
Subject: [PATCH 402/416] commander dummy node: set control velocity enabled
 flag

---
 src/platforms/ros/nodes/commander/commander.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index fee32b55f9..3829cbc32b 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -119,6 +119,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 		msg_vehicle_control_mode.flag_control_altitude_enabled = false;
 		msg_vehicle_control_mode.flag_control_climb_rate_enabled = false;
 		msg_vehicle_control_mode.flag_control_position_enabled = false;
+		msg_vehicle_control_mode.flag_control_velocity_enabled = false;
 
 		break;
 
@@ -132,6 +133,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 			msg_vehicle_control_mode.flag_control_altitude_enabled = true;
 			msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
 			msg_vehicle_control_mode.flag_control_position_enabled = true;
+			msg_vehicle_control_mode.flag_control_velocity_enabled = true;
 		} else {
 			msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL;
 			msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL;
@@ -141,6 +143,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 			msg_vehicle_control_mode.flag_control_altitude_enabled = true;
 			msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
 			msg_vehicle_control_mode.flag_control_position_enabled = false;
+			msg_vehicle_control_mode.flag_control_velocity_enabled = false;
 		}
 		break;
 	}

From a0ae5aeebb9eea7e90cf365d931c9a29790ebba1 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 14:23:03 +0100
Subject: [PATCH 403/416] commander dummy node: small fix for
 vehicle_control_mode

---
 src/platforms/ros/nodes/commander/commander.cpp | 12 +++++++-----
 src/platforms/ros/nodes/commander/commander.h   |  1 +
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index 3829cbc32b..2673122c70 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -50,18 +50,18 @@ Commander::Commander() :
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
 	_msg_parameter_update(),
-	_msg_actuator_armed()
+	_msg_actuator_armed(),
+	_msg_vehicle_control_mode()
 {
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
-	px4::vehicle_control_mode msg_vehicle_control_mode;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode based on (faked) stick positions*/
-	EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode);
-	msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+	EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode);
+	_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
 
 	/* fill actuator armed */
 	float arm_th = 0.95;
@@ -71,6 +71,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 		/* Check for disarm */
 		if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
 			_msg_actuator_armed.armed = false;
+			_msg_vehicle_control_mode.flag_armed = false;
 			msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY;
 		}
 
@@ -78,6 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 		/* Check for arm */
 		if (msg->r > arm_th && msg->z < (1 - arm_th)) {
 			_msg_actuator_armed.armed = true;
+			_msg_vehicle_control_mode.flag_armed = true;
 			msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
 		}
 	}
@@ -88,7 +90,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
 	msg_vehicle_status.is_rotary_wing = true;
 
-	_vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
+	_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
 	_actuator_armed_pub.publish(_msg_actuator_armed);
 	_vehicle_status_pub.publish(msg_vehicle_status);
 
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index f251f7c1a0..58b7257b72 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -74,5 +74,6 @@ protected:
 
 	px4::parameter_update _msg_parameter_update;
 	px4::actuator_armed _msg_actuator_armed;
+	px4::vehicle_control_mode _msg_vehicle_control_mode;
 
 };

From 211b58cd1a0b8903a4f1c7a6f9e4e51deff8e7e2 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 14:40:28 +0100
Subject: [PATCH 404/416] fix typo in comment

---
 .../mc_att_control_multiplatform/mc_att_control_base.cpp        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index 5577c5f03e..1f0d88bcd7 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -138,7 +138,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			// _publish_att_sp = true;
 		}
 
-		/* reset yaw setpint to current position if needed */
+		/* reset yaw setpoint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
 			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;

From 869d4115faaf6120715289b0e81ead14a2737553 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 16:02:58 +0100
Subject: [PATCH 405/416] ros mixer: remove debug output

---
 src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index 54f5fa78b6..0749c8e926 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -223,19 +223,15 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro
 	_n.getParamCached("mixer", mixer_name);
 	if (mixer_name == "x") {
 		_rotors = _config_index[0];
-		ROS_WARN("using x");
 	} else if (mixer_name == "+") {
 		_rotors = _config_index[1];
 	} else if (mixer_name == "e") {
 		_rotors = _config_index[2];
 	} else if (mixer_name == "w") {
 		_rotors = _config_index[3];
-		ROS_WARN("using w");
 	} else if (mixer_name == "i") {
 		_rotors = _config_index[4];
-		ROS_WARN("using i");
 	}
-	ROS_WARN("mixer_name %s", mixer_name.c_str());
 
 	// mix
 	mix();

From 990573aef3f875bb122f524c5f563248c2e96343 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 17:11:01 +0100
Subject: [PATCH 406/416] fix logic error in manual input node

---
 src/platforms/ros/nodes/manual_input/manual_input.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 7e1ae7a174..72f6e252f9 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -145,10 +145,8 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
 		return;
-
-	} else {
-		ROS_WARN("requested mode via joystick that is not implemented");
 	}
+
 }
 
 int main(int argc, char **argv)

From 76d2417cc53c493a90689582ebae9057a3987641 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 29 Jan 2015 19:02:43 +0100
Subject: [PATCH 407/416] fix comment

---
 src/platforms/ros/nodes/position_estimator/position_estimator.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h
index 4d396f39a9..da1fc2c5a8 100644
--- a/src/platforms/ros/nodes/position_estimator/position_estimator.h
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h
@@ -33,7 +33,7 @@
 
 /**
  * @file position_estimator.h
- * Dummy position estimator that forwards attitude from gazebo to px4 topic
+ * Dummy position estimator that forwards position from gazebo to px4 topic
  *
  * @author Thomas Gubler <thomasgubler@gmail.com>
 */

From 6a122ab643e8a541329803117fc481c04aed902b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 30 Jan 2015 12:24:33 +0100
Subject: [PATCH 408/416] ros att estimator dummy node: publish timestamp

---
 .../ros/nodes/attitude_estimator/attitude_estimator.cpp          | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index f744aa11c2..1d36e3d99c 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -136,6 +136,7 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
 	msg_v_att.pitchspeed = -(float)msg->angular_velocity.y;
 	msg_v_att.yawspeed = -(float)msg->angular_velocity.z;
 
+	msg_v_att.timestamp = px4::get_time_micros();
 	_vehicle_attitude_pub.publish(msg_v_att);
 }
 

From 82b8a42929e5bdcd53e6f8f7b2ee6995fbdf0707 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 10:53:48 +0100
Subject: [PATCH 409/416] fix timestamp problem in dummy pos estimator

---
 .../ros/nodes/position_estimator/position_estimator.cpp         | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
index fae304bf49..ed3a4efa5c 100644
--- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
+++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp
@@ -52,7 +52,7 @@ PositionEstimator::PositionEstimator() :
 	_n(),
 	_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)),
 	_vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)),
-	_startup_time(px4::get_time_micros())
+	_startup_time(1)
 {
 }
 

From 7c958a2cca90a6262dc491fe9ef86d85bacdf3da Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 10:57:56 +0100
Subject: [PATCH 410/416] multiplatform pos ctrl: fix division by zero

---
 .../mc_pos_control.cpp                             | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index aa23e0a60d..a46163e6f6 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -227,14 +227,14 @@ MulticopterPositionControl::reset_pos_sp()
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
 		/* shift position setpoint to make attitude setpoint continuous */
-		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
-				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
-		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
-				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
+		_pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
+				// - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
+				// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 
 		//XXX: port this once a mavlink like interface is available
 		// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
-		PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
+		PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
 
@@ -247,7 +247,7 @@ MulticopterPositionControl::reset_alt_sp()
 
 		//XXX: port this once a mavlink like interface is available
 		// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
-		PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2));
+		PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
 	}
 }
 
@@ -557,7 +557,7 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 	static uint64_t t_prev = 0;
 
 	uint64_t t = get_time_micros();
-	float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
+	float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f;
 	t_prev = t;
 
 	if (_control_mode->data().flag_armed && !was_armed) {

From 3b07890361d56ce80d881e3969ff097b5cd96af4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 11:57:31 +0100
Subject: [PATCH 411/416] update sitl default params, make posctrl very slow
 for now

---
 launch/ardrone.launch | 19 ++++++++-----------
 launch/iris.launch    | 17 ++++++-----------
 2 files changed, 14 insertions(+), 22 deletions(-)

diff --git a/launch/ardrone.launch b/launch/ardrone.launch
index 495388be5b..faf3bf6daf 100644
--- a/launch/ardrone.launch
+++ b/launch/ardrone.launch
@@ -3,17 +3,14 @@
 <include file="$(find px4)/launch/multicopter_x.launch" />
 
 <group ns="px4_multicopter">
-	<param name="MC_ROLL_P" type="double" value="6.0" />
-	<param name="MC_PITCH_P" type="double" value="6.0" />
-	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
-	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
-	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
-	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
-	<param name="MC_YAW_FF" type="double" value="0" />
-	<param name="MC_YAW_P" type="double" value="5.0" />
-	<param name="MC_YAWRATE_P" type="double" value="0.5" />
-	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
-	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
+	<param name="MPC_XY_P" type="double" value="1.0" />
+	<param name="MPC_XY_FF" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_P" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
 </group>
 
+  <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O multicopter"/>
+
 </launch>
diff --git a/launch/iris.launch b/launch/iris.launch
index 44a0ae0341..7b5b132508 100644
--- a/launch/iris.launch
+++ b/launch/iris.launch
@@ -3,18 +3,13 @@
 <include file="$(find px4)/launch/multicopter_w.launch" />
 
 <group ns="px4_multicopter">
-	<param name="MC_ROLL_P" type="double" value="6.0" />
-	<param name="MC_PITCH_P" type="double" value="6.0" />
-	<param name="MC_ROLLRATE_P" type="double" value="0.05" />
-	<param name="MC_ROLLRATE_D" type="double" value="0.0" />
-	<param name="MC_PITCHRATE_P" type="double" value="0.05" />
-	<param name="MC_PITCHRATE_D" type="double" value="0.0" />
-	<param name="MC_YAW_FF" type="double" value="0" />
-	<param name="MC_YAW_P" type="double" value="3.0" />
-	<param name="MC_YAWRATE_P" type="double" value="0.5" />
-	<param name="MC_MAN_R_MAX" type="double" value="10.0" />
-	<param name="MC_MAN_P_MAX" type="double" value="10.0" />
 	<param name="mixer" type="string" value="i" />
+	<param name="MPC_XY_P" type="double" value="1.0" />
+	<param name="MPC_XY_FF" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_P" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
+	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
+	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
 </group>
 
 </launch>

From 68e35ef0fd05504a42ed802f2a744dc253a6f747 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 12:19:42 +0100
Subject: [PATCH 412/416] remove unintended/leftover changes in test makefile

---
 makefiles/config_px4fmu-v2_test.mk | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index af80cae8fe..c360f722bb 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -66,10 +66,10 @@ MODULES		+= examples/publisher
 MODULES		+= modules/systemlib
 MODULES		+= modules/systemlib/mixer
 MODULES		+= modules/uORB
+LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/conversion
-LIBRARIES	+= lib/mathlib/CMSIS
 MODULES		+= platforms/nuttx
 
 #
@@ -92,6 +92,7 @@ MODULES		+= drivers/pca8574
 #
 # Tests
 #
+
 MODULES 	+= modules/unit_test
 MODULES		+= modules/mavlink/mavlink_tests
 MODULES 	+= modules/commander/commander_tests

From 0dc511a76b873ceba8cf0677dcd54cd0c566cb3b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 12:20:54 +0100
Subject: [PATCH 413/416] remove unintended/leftover changes in fmu2 default
 makefile

---
 makefiles/config_px4fmu-v2_default.mk | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 2ce7388446..aa2e01229a 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -31,7 +31,7 @@ MODULES		+= drivers/mb12xx
 MODULES		+= drivers/ll40ls
 # MODULES		+= drivers/trone
 MODULES		+= drivers/gps
-# MODULES		+= drivers/hil
+MODULES		+= drivers/hil
 # MODULES		+= drivers/hott
 # MODULES		+= drivers/hott/hott_telemetry
 # MODULES		+= drivers/hott/hott_sensors
@@ -77,7 +77,7 @@ MODULES 	+= modules/land_detector
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-# MODULES		+= modules/ekf_att_pos_estimator
+MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 
 #

From 2979d146422908e108cecfe37094c191521810ed Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 12:25:55 +0100
Subject: [PATCH 414/416] remove unneeded include

---
 src/include/containers/List.hpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp
index 5c0ba59fd3..13cbda9382 100644
--- a/src/include/containers/List.hpp
+++ b/src/include/containers/List.hpp
@@ -38,7 +38,6 @@
  */
 
 #pragma once
-#include <cstddef>
 
 template<class T>
 class __EXPORT ListNode

From 33a32fca142b33e8058ad579c73dfd7cf6b8e030 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 12:55:15 +0100
Subject: [PATCH 415/416] Tools/ros: add license info

---
 Tools/ros/docker/px4-ros-full/Dockerfile                 | 1 +
 Tools/ros/docker/px4-ros-full/README.md                  | 2 ++
 Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh | 2 ++
 Tools/ros/px4_ros_installation_ubuntu.sh                 | 1 +
 Tools/ros/px4_workspace_create.sh                        | 1 +
 Tools/ros/px4_workspace_setup.sh                         | 2 ++
 Tools/ros/vagrant/docker-host-base/Vagrantfile           | 4 +++-
 Tools/ros/vagrant/docker-host-base/config/docker-default | 3 +++
 Tools/ros/vagrant/docker-host-base/config/xsessionrc     | 1 +
 Tools/ros/vagrant/docker-host/Vagrantfile                | 2 ++
 Tools/ros/vagrant/px4-ros/Vagrantfile                    | 1 +
 11 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/Tools/ros/docker/px4-ros-full/Dockerfile b/Tools/ros/docker/px4-ros-full/Dockerfile
index 9c0fbc6886..1242b56b55 100644
--- a/Tools/ros/docker/px4-ros-full/Dockerfile
+++ b/Tools/ros/docker/px4-ros-full/Dockerfile
@@ -1,6 +1,7 @@
 #
 # PX4 full ROS container
 #
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 
 FROM ubuntu:14.04.1
 MAINTAINER Andreas Antener <andreas@uaventure.com>
diff --git a/Tools/ros/docker/px4-ros-full/README.md b/Tools/ros/docker/px4-ros-full/README.md
index 7d75754d9c..af5170c70a 100644
--- a/Tools/ros/docker/px4-ros-full/README.md
+++ b/Tools/ros/docker/px4-ros-full/README.md
@@ -2,6 +2,8 @@
 
 Full desktop ROS container.
 
+License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
 **TODO:**
 
 - use https://github.com/phusion/baseimage-docker as base
diff --git a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
index 231166e275..2de5f8bec9 100644
--- a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
+++ b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
@@ -3,6 +3,8 @@
 # Create workspace at current location and fetch source repositories
 #
 
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
 WDIR=`pwd`
 WORKSPACE=$WDIR/catkin_ws
 
diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh
index 3b8b29381f..7efc400cdd 100755
--- a/Tools/ros/px4_ros_installation_ubuntu.sh
+++ b/Tools/ros/px4_ros_installation_ubuntu.sh
@@ -1,4 +1,5 @@
 #!/bin/sh
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 
 # main ROS Setup
 # following http://wiki.ros.org/indigo/Installation/Ubuntu
diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh
index 4055f73201..cf80bcf8d6 100755
--- a/Tools/ros/px4_workspace_create.sh
+++ b/Tools/ros/px4_workspace_create.sh
@@ -1,5 +1,6 @@
 #!/bin/sh
 # this script creates a catkin_ws in the current folder
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 
 mkdir -p catkin_ws/src
 cd catkin_ws/src
diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh
index 1675e54f3b..53568b4fb0 100755
--- a/Tools/ros/px4_workspace_setup.sh
+++ b/Tools/ros/px4_workspace_setup.sh
@@ -1,4 +1,6 @@
 #!/bin/bash
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
 # run this script from the root of your catkin_ws
 source devel/setup.bash
 cd src
diff --git a/Tools/ros/vagrant/docker-host-base/Vagrantfile b/Tools/ros/vagrant/docker-host-base/Vagrantfile
index bc9329bf80..06e4e897d0 100644
--- a/Tools/ros/vagrant/docker-host-base/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host-base/Vagrantfile
@@ -9,6 +9,8 @@
 # After build, do "vagrant package --base docker-host-base" to package,
 # and import as box: "vagrant box add --name uaventure/docker-host-base package.box"
 #
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
 Vagrant.configure(2) do |config|
   config.vm.box = "ubuntu/trusty64"
 
@@ -19,7 +21,7 @@ Vagrant.configure(2) do |config|
     vb.gui = true
     vb.memory = "1024"
   end
-  
+
   config.vm.provision "file", source: "config/docker-default", destination: "/home/vagrant/docker-default"
   config.vm.provision "file", source: "config/xsessionrc", destination: "/home/vagrant/.xsessionrc"
 
diff --git a/Tools/ros/vagrant/docker-host-base/config/docker-default b/Tools/ros/vagrant/docker-host-base/config/docker-default
index e8a86ce6a1..19c466b6b0 100644
--- a/Tools/ros/vagrant/docker-host-base/config/docker-default
+++ b/Tools/ros/vagrant/docker-host-base/config/docker-default
@@ -23,4 +23,7 @@
 #export TMPDIR="/mnt/bigdrive/docker-tmp"
 
 # Expose TCP port in addition to socket
+
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
 DOCKER_OPTS="${DOCKER_OPTS} -H unix:///var/run/docker.sock -H 0.0.0.0:2375"
diff --git a/Tools/ros/vagrant/docker-host-base/config/xsessionrc b/Tools/ros/vagrant/docker-host-base/config/xsessionrc
index d8ab6d53da..1bd4f8d3f2 100644
--- a/Tools/ros/vagrant/docker-host-base/config/xsessionrc
+++ b/Tools/ros/vagrant/docker-host-base/config/xsessionrc
@@ -2,4 +2,5 @@
 #
 # Disable X access control so we can easily start GUI apps
 #
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 xhost +
diff --git a/Tools/ros/vagrant/docker-host/Vagrantfile b/Tools/ros/vagrant/docker-host/Vagrantfile
index 4ae94258d3..2a0a022717 100644
--- a/Tools/ros/vagrant/docker-host/Vagrantfile
+++ b/Tools/ros/vagrant/docker-host/Vagrantfile
@@ -12,6 +12,8 @@
 # You can then load an existing image, e.g.:
 # "docker load -i container-image.tar"
 #
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
 Vagrant.configure(2) do |config|
   config.vm.box = "uaventure/docker-host-base"
 
diff --git a/Tools/ros/vagrant/px4-ros/Vagrantfile b/Tools/ros/vagrant/px4-ros/Vagrantfile
index a122fb4a3b..5b372a94d0 100644
--- a/Tools/ros/vagrant/px4-ros/Vagrantfile
+++ b/Tools/ros/vagrant/px4-ros/Vagrantfile
@@ -23,6 +23,7 @@
 # - do not run the docker container with "--rm" (vagrant default). is that even possible?
 # - maybe map a local working directory to compile stuff without loosing it in side the docker container
 #
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 
 Vagrant.configure(2) do |config|
   # Configure docker host

From 143508529feb10d5e767794b1fdd236d6915901b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 1 Feb 2015 17:16:23 +0100
Subject: [PATCH 416/416] move rosbag launch and comment out

---
 launch/ardrone.launch     | 2 --
 launch/multicopter.launch | 1 +
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/launch/ardrone.launch b/launch/ardrone.launch
index faf3bf6daf..d53333b118 100644
--- a/launch/ardrone.launch
+++ b/launch/ardrone.launch
@@ -11,6 +11,4 @@
 	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
 </group>
 
-  <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O multicopter"/>
-
 </launch>
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 0f8cc5132d..95400bd82d 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -9,6 +9,7 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
+	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 
 </launch>