starting to port mc att ctrl multiplatform to the latest api

6 files changed, 209 insertions, 108 deletions

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 822a504b5..1e40c2b05 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 bff5289fd..95d608684 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 aff59778e..0de832df9 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 cf4c726a7..124147079 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_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
+	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_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 67ebe64cd..b356b5dc0 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 000000000..e982ae354
--- /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;
+}