lots' of header juggling and small changes to make mc att control compile for NuttX and ROS

20 files changed, 376 insertions, 88 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 489467db7..19a14f62a 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 f2e467e5a..910b785a3 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 38c5ebc7b..edf4fe1a0 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 bc23cdd18..666200390 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 ca702d63c..34137ee6f 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 2311e0a7c..012779646 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 806f5933a..1e76aae60 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 57b45e3ab..20f099831 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 e0bcbc6e9..53ab74305 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 13bff9561..906bd0a47 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 24009a5e6..1da738408 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 a1dca8a8c..be627866e 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 12187d70e..1b3ffd59f 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 b3431722f..012d8cb61 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 000000000..768bf7f53
--- /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 668d9dfdf..a9dfb13f8 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 a1bf9919e..712e0dd63 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 525f94aae..752c6b5fe 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 000000000..1e912d3ac
--- /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 000000000..a7ee6fd84
--- /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;
+}