fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:49:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros mavlink node: handle set_attitude_target

Browse Source
This commit is contained in:
Thomas Gubler 2015-02-12 20:43:52 +01:00
parent 3e5cbfcf77
commit 93f8fc33c8
2 changed files with 65 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/platforms/ros/nodes/mavlink/mavlink.cpp
View File

@ -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>("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);
}

26
src/platforms/ros/nodes/mavlink/mavlink.h
View File

@ -43,6 +43,7 @@
#include "ros/ros.h"
#include <mavconn/interface.h>
#include <px4/vehicle_attitude.h>
#include <px4/vehicle_attitude_setpoint.h>
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);
};
}