Merge pull request #1793 from PX4/ros_mavlink

ROS SITL: offboard setpoints

11 files changed, 767 insertions, 44 deletions

diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index f8561fa3b..0e98783fd 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -66,6 +66,8 @@
 #include <px4_vehicle_global_velocity_setpoint.h>
 #include <px4_vehicle_local_position.h>
 #include <px4_position_setpoint_triplet.h>
+#include <px4_offboard_control_mode.h>
+#include <px4_vehicle_force_setpoint.h>
 #endif
 
 #else
@@ -93,6 +95,8 @@
 #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>
+#include <platforms/nuttx/px4_messages/px4_offboard_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_force_setpoint.h>
 #endif
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index 2673122c7..0c32026f3 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -45,18 +45,25 @@
 Commander::Commander() :
 	_n(),
 	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+	_offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, 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)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
 	_msg_parameter_update(),
 	_msg_actuator_armed(),
-	_msg_vehicle_control_mode()
+	_msg_vehicle_control_mode(),
+	_msg_offboard_control_mode(),
+	_got_manual_control(false)
 {
+
+	/* Default to offboard control: when no joystick is connected offboard control should just work */
+
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
+	_got_manual_control = true;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode based on (faked) stick positions*/
@@ -101,12 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	}
 }
 
+void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode)
+{
+	msg_vehicle_control_mode.flag_control_manual_enabled = false;
+	msg_vehicle_control_mode.flag_control_offboard_enabled = true;
+	msg_vehicle_control_mode.flag_control_auto_enabled = false;
+
+	msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate ||
+		!msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+	msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude ||
+		!msg_offboard_control_mode.ignore_position ||
+		!msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_acceleration_force;
+
+
+	msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
+
+	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
+}
+
 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
 
+
+	if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON)
+	{
+		SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
+		return;
+	}
+
+	msg_vehicle_control_mode.flag_control_offboard_enabled = false;
+
 	switch (msg->mode_switch) {
 		case px4::manual_control_setpoint::SWITCH_POS_NONE:
 		ROS_WARN("Joystick button mapping error, main mode not set");
@@ -152,6 +198,35 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 
 }
 
+void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
+{
+	_msg_offboard_control_mode = *msg;
+
+	/* Force system into offboard control mode */
+	if (!_got_manual_control) {
+		SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
+		
+		px4::vehicle_status msg_vehicle_status;
+		msg_vehicle_status.timestamp = px4::get_time_micros();
+		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;
+		msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+		
+
+		_msg_actuator_armed.armed = true;
+		_msg_actuator_armed.timestamp = px4::get_time_micros();
+
+		_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+		_msg_vehicle_control_mode.flag_armed = true;
+		
+
+		_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");
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index 58b7257b7..c537c8419 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -44,6 +44,7 @@
 #include <px4/vehicle_status.h>
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
+#include <px4/offboard_control_mode.h>
 
 class Commander
 {
@@ -59,14 +60,26 @@ protected:
 	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
 	/**
+	 * Stores the offboard control mode
+	 */
+	void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &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);
 
+	/**
+	 * Sets offboard controll flags in msg_vehicle_control_mode
+	 */
+	void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Subscriber _offboard_control_mode_sub;
 	ros::Publisher _vehicle_control_mode_pub;
 	ros::Publisher _actuator_armed_pub;
 	ros::Publisher _vehicle_status_pub;
@@ -75,5 +88,8 @@ protected:
 	px4::parameter_update _msg_parameter_update;
 	px4::actuator_armed _msg_actuator_armed;
 	px4::vehicle_control_mode _msg_vehicle_control_mode;
+	px4::offboard_control_mode _msg_offboard_control_mode;
+
+	bool _got_manual_control;
 
 };
diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
new file mode 100644
index 000000000..fb0b09de1
--- /dev/null
+++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 demo_offboard_position_Setpoints.cpp
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "demo_offboard_attitude_setpoints.h"
+
+#include <platforms/px4_middleware.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Float64.h>
+#include <math.h>
+#include <tf/transform_datatypes.h>
+
+DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
+	_n(),
+	_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/attitude", 1)),
+	_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint/att_throttle", 1))
+{
+}
+
+
+int DemoOffboardAttitudeSetpoints::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (ros::ok()) {
+		loop_rate.sleep();
+		ros::spinOnce();
+
+		/* Publish example offboard attitude setpoint */
+		geometry_msgs::PoseStamped pose;
+		tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0);
+		quaternionTFToMsg(q, pose.pose.orientation);
+
+		_attitude_sp_pub.publish(pose);
+
+		std_msgs::Float64 thrust;
+		thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump'
+		_thrust_sp_pub.publish(thrust);
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "demo_offboard_position_setpoints");
+	DemoOffboardAttitudeSetpoints d;
+	return d.main();
+}
diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h
index 559659a1d..d7b7a37ba 100644
--- a/src/modules/uORB/topics/offboard_control_mode.h
+++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h
@@ -1,21 +1,20 @@
 /****************************************************************************
  *
- *	 Copyright (C) 2008-2015 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 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.
+ *    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
@@ -33,41 +32,27 @@
  ****************************************************************************/
 
 /**
- * @file offboard_control_mode.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_
-#define TOPIC_OFFBOARD_CONTROL_MODE_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Off-board control mode
- */
-
-/**
- * @addtogroup topics
- * @{
- */
+ * @file demo_offboard_attitude_Setpoints.h
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
 
-struct offboard_control_mode_s {
-	uint64_t timestamp;
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
 
-	bool ignore_thrust;
-	bool ignore_attitude;
-	bool ignore_bodyrate;
-	bool ignore_position;
-	bool ignore_velocity;
-	bool ignore_acceleration_force;
+class DemoOffboardAttitudeSetpoints
+{
+public:
+	DemoOffboardAttitudeSetpoints();
 
-}; /**< offboard control inputs */
-/**
- * @}
- */
+	~DemoOffboardAttitudeSetpoints() {}
 
-/* register this as object request broker structure */
-ORB_DECLARE(offboard_control_mode);
+	int main();
 
-#endif
+protected:
+	ros::NodeHandle _n;
+	ros::Publisher _attitude_sp_pub;
+	ros::Publisher _thrust_sp_pub;
+};
diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp
new file mode 100644
index 000000000..7366d7fc6
--- /dev/null
+++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 demo_offboard_position_Setpoints.cpp
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "demo_offboard_position_setpoints.h"
+
+#include <platforms/px4_middleware.h>
+#include <geometry_msgs/PoseStamped.h>
+
+DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() :
+	_n(),
+	_local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/local_position", 1))
+{
+}
+
+
+int DemoOffboardPositionSetpoints::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (ros::ok()) {
+		loop_rate.sleep();
+		ros::spinOnce();
+
+		/* Publish example offboard position setpoint */
+		geometry_msgs::PoseStamped pose;
+		pose.pose.position.x = 0;
+		pose.pose.position.y = 0;
+		pose.pose.position.z = 1;
+		_local_position_sp_pub.publish(pose);
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "demo_offboard_position_setpoints");
+	DemoOffboardPositionSetpoints d;
+	return d.main();
+}
diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h
new file mode 100644
index 000000000..7d39690f4
--- /dev/null
+++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 demo_offboard_position_Setpoints.h
+ *
+ * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+
+class DemoOffboardPositionSetpoints
+{
+public:
+	DemoOffboardPositionSetpoints();
+
+	~DemoOffboardPositionSetpoints() {}
+
+	int main();
+
+protected:
+	ros::NodeHandle _n;
+	ros::Publisher _local_position_sp_pub;
+};
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 72f6e252f..8488c518f 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -76,7 +76,8 @@ ManualInput::ManualInput() :
 	_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);
+	_n.param("map_auto_rtl", _param_buttons_map[5], 5);
+	_n.param("map_offboard", _param_buttons_map[6], 6);
 
 	/* Default to manual */
 	_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
@@ -84,6 +85,8 @@ ManualInput::ManualInput() :
 	_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;
+	_msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
 
 }
 
@@ -120,7 +123,6 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do
 
 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;
@@ -128,6 +130,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		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;
+		msg_mc_sp.offboard_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) {
@@ -136,6 +139,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		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;
+		msg_mc_sp.offboard_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) {
@@ -144,6 +148,15 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co
 		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;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+	} else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == 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;
+		msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
 		return;
 	}
 
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index bf704f675..2bafcca2e 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -77,6 +77,8 @@ protected:
 		MAIN_STATE_AUTO_MISSION,
 		MAIN_STATE_AUTO_LOITER,
 		MAIN_STATE_AUTO_RTL,
+		// MAIN_STATE_ACRO,
+		MAIN_STATE_OFFBOARD,
 		MAIN_STATE_MAX
 	};
 
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
new file mode 100644
index 000000000..5459fcffd
--- /dev/null
+++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp
@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 mavlink.cpp
+ * Dummy mavlink node that interfaces to a mavros node via UDP
+ * This simulates the onboard mavlink app to some degree. It should be possible to
+ * send offboard setpoints via mavros to the SITL setup the same way as on the real system
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "mavlink.h"
+
+#include <platforms/px4_middleware.h>
+
+using namespace px4;
+
+Mavlink::Mavlink() :
+	_n(),
+	_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
+	_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
+	_v_att_sp_pub(_n.advertise<vehicle_attitude_setpoint>("vehicle_attitude_setpoint", 1)),
+	_pos_sp_triplet_pub(_n.advertise<position_setpoint_triplet>("position_setpoint_triplet", 1)),
+	_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)),
+	_force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1))
+{
+	_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
+	_link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3));
+
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "mavlink");
+	Mavlink m;
+	ros::spin();
+	return 0;
+}
+
+void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_attitude_quaternion_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->q[0],
+			msg->q[1],
+			msg->q[2],
+			msg->q[3],
+			msg->rollspeed,
+			msg->pitchspeed,
+			msg->yawspeed);
+	_link->send_message(&msg_m);
+}
+
+void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_local_position_ned_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->x,
+			msg->y,
+			msg->z,
+			msg->vx,
+			msg->vy,
+			msg->vz);
+	_link->send_message(&msg_m);
+}
+
+void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
+	(void)sysid;
+	(void)compid;
+
+	switch(mmsg->msgid) {
+		case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+			handle_msg_set_attitude_target(mmsg);
+			break;
+		case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+			handle_msg_set_position_target_local_ned(mmsg);
+			break;
+		default:
+			break;
+	}
+
+}
+
+void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg)
+{
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target);
+
+	static offboard_control_mode offboard_control_mode;
+
+	/* set correct ignore flags for thrust field: copy from mavlink message */
+	offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
+
+	/*
+	 * if attitude or body rate have been used (not ignored) previously and this message only sends
+	 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
+	 * body rates to keep the controllers running
+	 */
+	bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
+	bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+
+	if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) {
+		/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
+		offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate;
+		offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude;
+	} else {
+		offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
+		offboard_control_mode.ignore_attitude = ignore_attitude;
+	}
+	offboard_control_mode.ignore_position = true;
+	offboard_control_mode.ignore_velocity = true;
+	offboard_control_mode.ignore_acceleration_force = true;
+
+	offboard_control_mode.timestamp = get_time_micros();
+	_offboard_control_mode_pub.publish(offboard_control_mode);
+
+	static vehicle_attitude_setpoint att_sp = {};
+
+	/* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint
+	 * gets published only if in offboard mode. We leave that out for now.
+	 */
+
+	att_sp.timestamp = get_time_micros();
+	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, (float(*)[3])att_sp.R_body.data());
+	att_sp.R_valid = true;
+
+	if (!offboard_control_mode.ignore_thrust) {
+		att_sp.thrust = set_attitude_target.thrust;
+	}
+
+	if (!offboard_control_mode.ignore_attitude) {
+		for (ssize_t i = 0; i < 4; i++) {
+			att_sp.q_d[i] = set_attitude_target.q[i];
+		}
+		att_sp.q_d_valid = true;
+	}
+
+	_v_att_sp_pub.publish(att_sp);
+
+
+	//XXX real mavlink publishes rate sp here
+
+}
+
+void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg)
+{
+
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned);
+
+	offboard_control_mode offboard_control_mode;
+	// memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	// XXX removed for sitl, makes maybe sense to re-introduce at some point
+	// if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				// set_position_target_local_ned.target_system == 0) &&
+			// (mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 // set_position_target_local_ned.target_component == 0)) {
+
+	/* convert mavlink type (local, NED) to uORB offboard control struct */
+	offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
+	offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
+	offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
+	bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+	/* yaw ignore flag mapps to ignore_attitude */
+	offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
+	/* yawrate ignore flag mapps to ignore_bodyrate */
+	offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
+
+
+
+	offboard_control_mode.timestamp = get_time_micros();
+	_offboard_control_mode_pub.publish(offboard_control_mode);
+
+	/* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet
+	 * gets published only if in offboard mode. We leave that out for now.
+	 */
+	if (is_force_sp && offboard_control_mode.ignore_position &&
+			offboard_control_mode.ignore_velocity) {
+		/* The offboard setpoint is a force setpoint only, directly writing to the force
+		 * setpoint topic and not publishing the setpoint triplet topic */
+		vehicle_force_setpoint	force_sp;
+		force_sp.x = set_position_target_local_ned.afx;
+		force_sp.y = set_position_target_local_ned.afy;
+		force_sp.z = set_position_target_local_ned.afz;
+		//XXX: yaw
+
+		_force_sp_pub.publish(force_sp);
+	} else {
+		/* It's not a pure force setpoint: publish to setpoint triplet  topic */
+		position_setpoint_triplet pos_sp_triplet;
+		pos_sp_triplet.previous.valid = false;
+		pos_sp_triplet.next.valid = false;
+		pos_sp_triplet.current.valid = true;
+		pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others
+
+		/* set the local pos values */
+		if (!offboard_control_mode.ignore_position) {
+			pos_sp_triplet.current.position_valid = true;
+			pos_sp_triplet.current.x = set_position_target_local_ned.x;
+			pos_sp_triplet.current.y = set_position_target_local_ned.y;
+			pos_sp_triplet.current.z = set_position_target_local_ned.z;
+		} else {
+			pos_sp_triplet.current.position_valid = false;
+		}
+
+		/* set the local vel values */
+		if (!offboard_control_mode.ignore_velocity) {
+			pos_sp_triplet.current.velocity_valid = true;
+			pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
+			pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
+			pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
+		} else {
+			pos_sp_triplet.current.velocity_valid = false;
+		}
+
+		/* set the local acceleration values if the setpoint type is 'local pos' and none
+		 * of the accelerations fields is set to 'ignore' */
+		if (!offboard_control_mode.ignore_acceleration_force) {
+			pos_sp_triplet.current.acceleration_valid = true;
+			pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
+			pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
+			pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
+			pos_sp_triplet.current.acceleration_is_force =
+				is_force_sp;
+
+		} else {
+			pos_sp_triplet.current.acceleration_valid = false;
+		}
+
+		/* set the yaw sp value */
+		if (!offboard_control_mode.ignore_attitude) {
+			pos_sp_triplet.current.yaw_valid = true;
+			pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
+
+		} else {
+			pos_sp_triplet.current.yaw_valid = false;
+		}
+
+		/* set the yawrate sp value */
+		if (!offboard_control_mode.ignore_bodyrate) {
+			pos_sp_triplet.current.yawspeed_valid = true;
+			pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
+
+		} else {
+			pos_sp_triplet.current.yawspeed_valid = false;
+		}
+		//XXX handle global pos setpoints (different MAV frames)
+
+		_pos_sp_triplet_pub.publish(pos_sp_triplet);
+	}
+}
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h
new file mode 100644
index 000000000..acb2408f3
--- /dev/null
+++ b/src/platforms/ros/nodes/mavlink/mavlink.h
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 mavlink.h
+ * Dummy mavlink node that interfaces to a mavros node via UDP
+ * This simulates the onboard mavlink app to some degree. It should be possible to
+ * send offboard setpoints via mavros to the SITL setup the same way as on the real system
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <mavconn/interface.h>
+#include <px4/vehicle_attitude.h>
+#include <px4/vehicle_local_position.h>
+#include <px4/vehicle_attitude_setpoint.h>
+#include <px4/position_setpoint_triplet.h>
+#include <px4/vehicle_force_setpoint.h>
+#include <px4/offboard_control_mode.h>
+
+namespace px4
+{
+
+class Mavlink
+{
+public:
+	Mavlink();
+
+	~Mavlink() {}
+
+protected:
+
+	ros::NodeHandle _n;
+	mavconn::MAVConnInterface::Ptr _link;
+	ros::Subscriber _v_att_sub;
+	ros::Subscriber _v_local_pos_sub;
+	ros::Publisher _v_att_sp_pub;
+	ros::Publisher _pos_sp_triplet_pub;
+	ros::Publisher _offboard_control_mode_pub;
+	ros::Publisher _force_sp_pub;
+
+	/**
+	 *
+	 * Simulates output of attitude data from the FCU
+	 * Equivalent to the mavlink stream ATTITUDE_QUATERNION
+	 *
+	 * */
+	void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg);
+
+	/**
+	 *
+	 * Simulates output of local position data from the FCU
+	 * Equivalent to the mavlink stream LOCAL_POSITION_NED
+	 *
+	 * */
+	void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg);
+
+
+	/**
+	 *
+	 * Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver")
+	 *
+	 * */
+	void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid);
+
+	/**
+	 *
+	 * Handle SET_ATTITUDE_TARGET mavlink messages
+	 *
+	 * */
+	void handle_msg_set_attitude_target(const mavlink_message_t *mmsg);
+
+	/**
+	 *
+	 * Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages
+	 *
+	 * */
+	void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg);
+
+};
+
+}