前面一篇讲了机载电脑怎么接受飞控的px4消息.这一篇讲解如何飞控怎么接收从机载电脑传过来的消息.分成两部分:机载电脑发送消息,飞控接收消息.
pixhawk版本:pixhawk4
px4版本:1.11.2
ros版本:1.14.10
机载电脑:TX2 ubuntu18系统
mavros发送消息
1.创建msg消息
参考前一篇文章的文件路径,首先需要在机载电脑中的~/catkin_ws/src/mavros/mavros_msgs/msg文件夹下新建Tx2_to_Vehicle.msg文件,该文件就是我们要传输的消息的数据类型,然后在文件中写入:
std_msgs/Header header
float32[3] f_a
float32[3] tau_a
然后在~/catkin_ws/src/mavros/mavros_msgs目录下的CmakeLists.txt文件中添加Tx2_to_Vehicle.msg文件
add_message_files(
....
Tx2_to_Vehicle.msg
)
然后使用:catkin build 命令在~/catkin_ws目录下编译一下,确保在mavros_msgs目录下生成了Tx2_to_Vehicle.h头文件,方便后续文件的引入.
2.添加自定义的mavros消息
修改~/catkin_ws/src/mavlink/message_definitions/v1.0/common.xml文件,添加自定义消息:
<message id="14226" name="TX2_TO_VEHICLE">
<description>the message from tx2 to vehicle</description>
<field type="uint64_t" name="time_usec" units="us">Timestamp</field>
<field type="float[3]" name="f_a">float32_array</field>
<field type="float[3]" name="tau_a">float32_array</field>
</message>
在~/catkin_ws/src/mavlink/message_definitions目录下,使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件:
python3 -m mavgenerate
然后在图形界面中选择
XML:~/catkin_ws/src/mavlink/message_definitions/v1.0/standard.xml
OUT:~/catkin_ws/src/mavlink/message_definitions/v1.0
Language:C
Protocol:1.0
Validate:勾选
点击Generate按钮,在~/catkin_ws/src/mavlink/message_definitions/v1.0/中会生成common和standard文件夹。
为了将消息发送出去,我们需要编写一个插件.~/catkin_ws/src/mavros/mavros_extras/src/plugins目录下创建Tx2_to_Vehicle.cpp文件
#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <mavros_msgs/Tx2_to_Vehicle.h>
namespace mavros {
namespace extra_plugins{
class Tx2ToVehiclePlugin : public plugin::PluginBase {
public:
Tx2ToVehiclePlugin() : PluginBase(),
nh("~Tx2_to_Px4"){ };
void initialize(UAS &uas_)
{
PluginBase::initialize(uas_);
mavros2fcu_sub = nh.subscribe("send_data", 10, &Tx2ToVehiclePlugin::Tx2ToVehicle_cb, this);
};
Subscriptions get_subscriptions()
{
return {/* RX disabled */ };
}
private:
ros::NodeHandle nh;
ros::Subscriber send_sub;
void Tx2ToVehicle_cb(const mavros_msgs::Tx2_to_Vehicle::ConstPtr &req)
{
mavros::UAS *m_uas_ = static_cast<Tx2ToVehiclePlugin *>(this)->m_uas;
mavlink::common::msg:: TX2_TO_VEHICLE send_data = {};
//官方教程中没有这句,所以会报错,因为send_message_ignore_drop的参数是message消息类型,
//所以需要先用python mavgenerate.py生成mavlink::common::msg::KEY_COMMANDS
send_data.f_a[0] = req->f_a[0];
send_data.f_a[1] = req->f_a[1];
send_data.f_a[2] = req->f_a[2];
send_data.tau_a[0] = req->tau_a[0];
send_data.tau_a[1] = req->tau_a[1];
send_data.tau_a[2] = req->tau_a[2];
//std::cout << "Got data : " << req->a << req->b << req->c << std::endl;
UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
}
};
} // namespace extra_plugins
} // namespace mavros
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::Tx2ToVehiclePlugin, mavros::plugin::PluginBase)
将自定义的插件添加到插件列表中,用于MAVROS自启动插件:
~/catkin_ws/src/mavros/mavros_extras/mavros_plugins.xml中添加:
<class name="mavros2fcu" type="mavros::extra_plugins::Tx2ToVehiclePlugin" base_class_type="mavros::plugin::PluginBase">
<description>send mavros to fcu</description>
</class>
在~/catkin_ws/src/mavros/mavros_extras/CMmakeLists.txt添加编译信息
add_library(mavros_extras
src/plugins/Tx2_to_Vehicle.cpp
)
5.编译
在工作空间catkin_ws下执行命令:
catkin build
6.手动发布数据
6.1创建ros工作空间
插件Tx2_to_Vehicle.cpp中会订阅数据并发布出去,最开始时是订阅不到数据的,因为数据还没有从其他的模块中发布,为了演示程序的正确性,所以我们这里需要手动的发布一些数据,让这个模块订阅然后发送给飞控.步骤如下:
mkdir ros_demo //创建工作空间
cd ros_demo
mkdir src //创建子空间
cd src
catkin_create_pkg demo //创建包名,该命令能够在src/demo目录下生成package.xml和CMakeLists.txt
6.2新建发布文件
在demo目录下新建一个pub_data.cpp文件,在该文件中写入:
#include <time.h>
#include <ros/ros.h>
#include <mavros_msgs/Mavros2Fcu.h>
using namespace std;
mavros_msgs::Mavros2Fcu data;
int main(int argc, char **argv){
ros::init(argc, argv, "ss");
ros::NodeHandle nh;
float aa = 0;
data.f_a[0] = aa+1;
data.f_a[1] = aa+2;
data.f_a[2] = aa+3;
data.tau_a[0] = aa+1;
data.tau_a[1] = aa+2;
data.tau_a[2] = aa+3;
ros::Publisher test;
test = nh.advertise<mavros_msgs::Tx2_to_Vehicle>("/mavros/Tx2_to_Px4/send_data", 10);
ros::Rate rate(20.0);
while(ros::ok()){
ros::spinOnce();
rate.sleep();
test.publish(data);
aa = aa + 1;
//cout<<data.a<<endl;
}
return 0;
}
该文件能够发布Tx2_to_Vehicle类型的消息,以便于Tx2_to_Vehicle.cpp这个插件订阅.
6.3配置package.xml和CMakeLists.txt
写好发布文件之后,还需要在package.xml和CMakeLists.txt中配置一下才能使用.
在CmakeLists.txt文件中添加:
find_package(catkin REQUIRED COMPONENTS
mavros
roscpp
std_msgs
mavros_msgs
)
add_executable(a src/pub_data.cpp) //a是文件执行名,可以随便取
target_link_libraries(a ${catkin_LIBRARIES})
在package.xml中添加:
<buildtool_depend>catkin</buildtool_depend>
<build_depend>mavros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>mavros_msgs</build_depend>
<build_export_depend>mavros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>mavros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>mavros_msgs</exec_depend>
最后编译一下:catkin_make
px4接收消息
1.添加uorb消息
在飞控端接收机载电脑传来的数据,需要定义uorb消息来接收.在~/Firmware/msg目录下创建tx2_to_vehicle.msg文件,并写入:
uint64 timestamp # time since system start (microseconds)
float32[3] f_a
float32[3] tau_a
然后在~/Firmware/msg目录下的CMakeLists.txt文件中添加tx2_to_vehicle.msg文件
set(msg_files
...
...
tx2_to_vehicle.msg
)
2.自定义mavlink消息
在~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/common.xml文件中,添加自定义消息
<message id="14226" name="TX2_TO_VEHICLE">
<description>receive data from tx2 to vehicle</description>
<field type="uint64_t" name="time_usec" units="us">Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.</field>
<field type="float[3]" name="f_a">float32_array</field>
<field type="float[3]" name="tau_a">float32_array</field>
</message>
这里的id和mavros端的id一致.
3.生成mavlink库文件
使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件,在~/Firmware/mavlink/include/mavlink/v2.0目录下打开终端并执行:
python3 -m mavgenerate
然后在打开的图形界面中执行下面的操作:
XML:~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml
OUT:~/Firmware/mavlink/include/mavlink/v2.0
Language:C
Protocol:2.0
Validate:勾选
点击Generate按钮,在~/Firmware/mavlink/include/mavlink/v2.0/中会生成common和standard文件夹。
可以在~/Firmware/mavlink/include/mavlink/v2.0/中生成的common文件夹中能找到生成的.h文件:
mavlink_msg_tx2_to_vehicle.h
4.修改mavlink_receiver.h文件
在~/Firmware/src/modules/mavlink目录下的mavlink_receiver.h中添加下面内容:
...
#include <uORB/topics/tx2_to_vehicle.h>
...
class MavlinkReceiver
{
...
private:
void handle_message_tx2_to_vehicle(mavlink_message_t *msg);
...
orb_advert_t _tx2_to_vehicle_pub{nullptr};
在~/Firmware/src/modules/mavlink目录下的mavlink_receiver.cpp文件中添加:
...
void MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
...
case MAVLINK_MSG_ID_TX2_TO_VEHICLE:
handle_message_tx2_to_vehicle(msg);
break;
...
}
...
void MavlinkReceiver::handle_message_tx2_to_vehicle(mavlink_message_t *msg){
mavlink_tx2_to_vehicle_t man;
mavlink_msg_tx2_to_vehicle_decode(msg, &man);
struct tx2_to_vehicle_s data = {};
data.timestamp = hrt_absolute_time();
data.f_a[0] = man.f_a[0];
data.f_a[1] = man.f_a[1];
data.f_a[2] = man.f_a[2];
data.tau_a[0] = man.tau_a[0];
data.tau_a[1] = man.tau_a[1];
data.tau_a[2] = man.tau_a[2];
if (_tx2_to_vehicle_pub == nullptr) {
_tx2_to_vehicle_pub = orb_advertise(ORB_ID(tx2_to_vehicle), &data);
} else {
orb_publish(ORB_ID(tx2_to_vehicle), _tx2_to_vehicle_pub, &data);
}
}
5.订阅模块
在~/Firmware/src/modules目录下创建一个文件夹receive_data.并在该文件夹下创建CMakeLists.txt文件和receive_data.cpp文件.在receive_data.cpp文件中写入:
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/tx2_to_vehicle.h>
extern "C" __EXPORT int receive_data_main(int argc, char **argv);
int receive_data_main(int argc, char **argv)
{
int data_sub_fd = orb_subscribe(ORB_ID(tx2_to_vehicle));
orb_set_interval(data_sub_fd, 200); // limit the update rate to 200ms
px4_pollfd_struct_t fds[1];
fds[0].fd = data_sub_fd, fds[0].events = POLLIN;
int error_counter = 0;
while(true)
{
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret == 0)
{
PX4_ERR("Got no data within a second");
}
else if (poll_ret < 0)
{
if (error_counter < 10 || error_counter % 50 == 0)
{
PX4_ERR("ERROR return value from poll(): %d", poll_ret);
}
error_counter++;
}
else
{
if (fds[0].revents & POLLIN)
{
struct tx2_to_vehicle_s input;
orb_copy(ORB_ID(tx2_to_vehicle), data_sub_fd, &input);
PX4_INFO("Recieved data a,b,c : %lf %lf %lf", (double)input.f_a[0], (double)input.f_a[1], (double)input.f_a[2]);
}
}
}
return 0;
}
在CMakeLists.txt文件中添加:
px4_add_module(
MODULE modules__receive_data
MAIN receive_data
SRCS
receive_data.cpp
DEPENDS
)
把该模块添加到系统的配置文件中去,在~/Firmware/boards/px4/fmu-v5/default.cmake中添加:
MODULES
...
receive_data
...
也可以添加到~/Firmware/boards/px4/sitl/default.cmake中,可以放到模拟仿真中去验证.
6.编译Firmware
刷新固件,用USB连上飞控板,在Firmware中执行:
make px4_fmu-ve uplod
测试
在机载电脑端终端中启动roscore,再另外打开一个终端执行roslaunch mavros px4.launch,另外打开一个终端,执行rostopic echo /mavros/state查看connected是否为true,为true表示连接上了,反之则没有。
连接上之后就可以执行rosrun demo pub_data发送数据。
用USB连接上地面站,在地面站中执行receive_data来收集数据,如下图:
当关闭rosrun demo pub_data后就会出现Got no data within a second的错误,表明数据已经发过来了,机载电脑端停止发送后,飞控就收不到数据。
以上就是px4接收自定义mavros消息的内容.