dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ros: mavlink dummy node: listen to vehicle local position and publish to mavlink (LOCAL_POSITION_NED)

This commit is contained in:
Thomas Gubler 2015-02-15 11:40:07 +01:00
parent 8d36305f8b
commit ca250d21eb
2 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,7 @@ 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)),
_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1))
{
_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
@ -71,7 +72,7 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
_link->get_system_id(),
_link->get_component_id(),
_link->get_channel(),
&msg_m, //XXX hardcoded
&msg_m,
get_time_micros() / 1000,
msg->q[0],
msg->q[1],
@ -83,6 +84,24 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
_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;

12
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_local_position.h>
#include <px4/vehicle_attitude_setpoint.h>
#include <px4/offboard_control_mode.h>
@ -61,17 +62,26 @@ 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 _offboard_control_mode_pub;
/**
*
* Simulates output of attitude data from the FCU
* Equivalent to the mavlink stream ATTITUDE
* 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);
/**
*