【PX4-AutoPilot教程-TIPS】PX4中MAVLink话题频率修改

环境:

PX4 :1.13.0

方法一:使用QGC地面站通过命令行解释器MAVLink Shell修改话题频率

连接PX4飞控后,打开QGC地面站,进入【Analyze Tools】,选择【MAVLink 检测】界面。

在这里插入图片描述

切换到【MAVLink 控制台】界面,输入以下指令修改话题频率。

mavlink stream -d /dev/ttyACM0 -s BATTERY_STATUS -r 100

在这里插入图片描述

其中,-d参数指定串口设备名称,-s参数指定话题名称,-r参数指定频率。

/dev/ttyACM0为USB接口,具体修改哪个接口需要根据实际情况修改,比如USART1对应的串口设备名称是/dev/ttyS0USART1不一定对应TELEM 1,这个是在default.px4board文件中有相关的定义。

CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"

BATTERY_STATUS为【MAVLink 检测】界面中的话题名称。

之后可以再次查看【MAVLink 检测】界面,可以看到BATTERY_STATUS话题的频率已经修改为100Hz。

在这里插入图片描述

发送频率同时受飞控硬件条件和数据链路带宽限制,实际频率不一定能达到设定值。

使用这种方法在飞控重启后会恢复成默认速率,需要重新设置。

方法二:使用SD卡中的命令脚本文件修改话题频率

在SD卡根目录下创建/etc/extras.txt文件,编辑加入以下内容:

mavlink start -d /dev/ttyACM0 -b 921600
mavlink stream -d /dev/ttyACM0 -s BATTERY_STATUS -r 100

这样做相当于在每次启动飞控时执行修改话题频率的命令,将SD卡插回飞控。

之后可以查看【MAVLink 检测】界面,可以看到BATTERY_STATUS话题的频率已经修改为100Hz。

在这里插入图片描述

注意,飞控上电后,USB接口只有接入电脑或机载电脑后才启动,飞控先启动再接入USB接口,将导致在执行该脚本文件时指令执行失败,设置发送频率不成功。而串口无论是否接入设备会直接启动。

方法三:通过修改PX4飞控固件源码修改话题频率

修改/src/modules/mavlink文件目录下mavlink_main.cpp文件。

使用USB获取话题数据修改MAVLINK_MODE_CONFIG下的设置,找到对应的话题名称,后面对应的数字为预设发送频率。

case MAVLINK_MODE_CONFIG: // USB
		// Note: streams requiring low latency come first
		configure_stream_local("TIMESYNC", 10.0f);
		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
		configure_stream_local("LOCAL_POSITION_NED", 30.0f);
		configure_stream_local("DISTANCE_SENSOR", 10.0f);
		configure_stream_local("MOUNT_ORIENTATION", 10.0f);
		configure_stream_local("ODOMETRY", 30.0f);

		configure_stream_local("ACTUATOR_CONTROL_TARGET0", 30.0f);
		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
		configure_stream_local("ALTITUDE", 10.0f);
		configure_stream_local("ATTITUDE", 50.0f);
		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
		configure_stream_local("ATTITUDE_TARGET", 8.0f);
		configure_stream_local("BATTERY_STATUS", 0.5f);
		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
		configure_stream_local("COLLISION", unlimited_rate);
		configure_stream_local("EFI_STATUS", 10.0f);
		configure_stream_local("ESC_INFO", 10.0f);
		configure_stream_local("ESC_STATUS", 10.0f);
		configure_stream_local("ESTIMATOR_STATUS", 5.0f);
		configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
		configure_stream_local("GPS2_RAW", unlimited_rate);
		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
		configure_stream_local("GPS_RAW_INT", unlimited_rate);
		configure_stream_local("GPS_STATUS", 1.0f);
		configure_stream_local("HIGHRES_IMU", 50.0f);
		configure_stream_local("HOME_POSITION", 0.5f);
		configure_stream_local("HYGROMETER_SENSOR", 1.0f);
		configure_stream_local("MAG_CAL_REPORT", 1.0f);
		configure_stream_local("MANUAL_CONTROL", 5.0f);
		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
		configure_stream_local("PING", 1.0f);
		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
		configure_stream_local("RAW_RPM", 5.0f);
		configure_stream_local("RC_CHANNELS", 10.0f);
		configure_stream_local("SCALED_IMU", 25.0f);
		configure_stream_local("SCALED_IMU2", 25.0f);
		configure_stream_local("SCALED_IMU3", 25.0f);
		configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
		configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
		configure_stream_local("SYS_STATUS", 1.0f);
		configure_stream_local("SYSTEM_TIME", 1.0f);
		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
		configure_stream_local("VFR_HUD", 20.0f);
		configure_stream_local("VIBRATION", 2.5f);
		configure_stream_local("WIND_COV", 10.0f);

使用串口获取话题数据修改MAVLINK_MODE_ONBOARD下的设置,找到对应的话题名称,后面对应的数字为预设发送频率。

case MAVLINK_MODE_ONBOARD:
		// Note: streams requiring low latency come first
		configure_stream_local("TIMESYNC", 10.0f);
		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
		configure_stream_local("HIGHRES_IMU", 50.0f);
		configure_stream_local("LOCAL_POSITION_NED", 30.0f);
		configure_stream_local("ATTITUDE", 100.0f);
		configure_stream_local("ALTITUDE", 10.0f);
		configure_stream_local("DISTANCE_SENSOR", 10.0f);
		configure_stream_local("ESC_INFO", 10.0f);
		configure_stream_local("ESC_STATUS", 10.0f);
		configure_stream_local("MOUNT_ORIENTATION", 10.0f);
		configure_stream_local("OBSTACLE_DISTANCE", 10.0f);
		configure_stream_local("ODOMETRY", 30.0f);

		configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f);
		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
		configure_stream_local("ATTITUDE_TARGET", 10.0f);
		configure_stream_local("BATTERY_STATUS", 0.5f);
		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
		configure_stream_local("COLLISION", unlimited_rate);
		configure_stream_local("EFI_STATUS", 2.0f);
		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
		configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
		configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
		configure_stream_local("GPS2_RAW", unlimited_rate);
		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
		configure_stream_local("GPS_RAW_INT", unlimited_rate);
		configure_stream_local("GPS_STATUS", 1.0f);
		configure_stream_local("HOME_POSITION", 0.5f);
		configure_stream_local("HYGROMETER_SENSOR", 1.0f);
		configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
		configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
		configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
		configure_stream_local("PING", 1.0f);
		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
		configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
		configure_stream_local("RAW_RPM", 5.0f);
		configure_stream_local("RC_CHANNELS", 20.0f);
		configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
		configure_stream_local("SYS_STATUS", 5.0f);
		configure_stream_local("SYSTEM_TIME", 1.0f);
		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
		configure_stream_local("VFR_HUD", 10.0f);
		configure_stream_local("VIBRATION", 0.5f);
		configure_stream_local("WIND_COV", 10.0f);

这里修改的是ONBOARD模式下的预设发送频率,所以在QGC地面站中配置MAVlink模式时也需将相关的MAVlink端口模式配置为ONBOARD,如下图所示。

在这里插入图片描述

修改好预设发送频率后,重新编译飞控固件并烧录至飞控板,重新连接QGC地面站,即可看到修改后的数据发送频率。


参考资料:

PX4 Mavros Topic 消息频率修改

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

后厂村路练习生

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值