1 files changed, 296 insertions, 0 deletions
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);
+	}
+}