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) */
 
+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;
 };
 
 /**
@@ -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