From 93f8fc33c890bb961f0fba03537cc54bf8a88d1f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 12 Feb 2015 20:43:52 +0100 Subject: ros mavlink node: handle set_attitude_target --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 43 +++++++++++++++++++++++++++-- src/platforms/ros/nodes/mavlink/mavlink.h | 26 ++++++++++++++++- 2 files changed, 65 insertions(+), 4 deletions(-) (limited to 'src/platforms') diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 131a4930f..a7830e97f 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -48,10 +48,12 @@ using namespace px4; Mavlink::Mavlink() : _n(), - _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) + _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_att_sp_pub(_n.advertise("vehicle_attitude_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) @@ -62,7 +64,7 @@ int main(int argc, char **argv) return 0; } -void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) +void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( @@ -80,3 +82,38 @@ void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) msg->yawspeed); _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; + default: + break; + } + +} + +void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) +{ + mavlink_set_attitude_target_t set_att_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); + + vehicle_attitude_setpoint msg; + + msg.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &msg.roll_body, &msg.pitch_body, &msg.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])msg.R_body.data()); + msg.R_valid = true; + msg.thrust = set_att_target.thrust; + for (ssize_t i = 0; i < 4; i++) { + msg.q_d[i] = set_att_target.q[i]; + } + msg.q_d_valid = true; + + _v_att_sp_pub.publish(msg); + +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index e683597a9..6f7536436 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -43,6 +43,7 @@ #include "ros/ros.h" #include #include +#include namespace px4 { @@ -59,8 +60,31 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; + ros::Publisher _v_att_sp_pub; + + /** + * + * Simulates output of attitude data from the FCU + * Equivalent to the mavlink stream ATTITUDE + * + * */ + void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &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); - void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg); }; } -- cgit v1.2.3