PX4模块设计之四十七：mavlink模块
Posted lida2003
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了PX4模块设计之四十七：mavlink模块相关的知识，希望对你有一定的参考价值。
PX4模块设计之四十七：mavlink模块
1. mavlink模块简介
### Description
This module implements the MAVLink protocol, which can be used on a Serial link or UDP network connection.
It communicates with the system via uORB: some messages are directly handled in the module (eg. mission
protocol), others are published via uORB (eg. vehicle_command).
Streams are used to send periodic messages with a specific rate, such as the vehicle attitude.
When starting the mavlink instance, a mode can be specified, which defines the set of enabled streams with their rates.
For a running instance, streams can be configured via `mavlink stream` command.
There can be multiple independent instances of the module, each connected to one serial device or network port.
### Implementation
The implementation uses 2 threads, a sending and a receiving thread. The sender runs at a fixed rate and dynamically
reduces the rates of the streams if the combined bandwidth is higher than the configured rate (`-r`) or the
physical link becomes saturated. This can be checked with `mavlink status`, see if `rate mult` is less than 1.
**Careful**: some of the data is accessed and modified from both threads, so when changing code or extend the
functionality, this needs to be take into account, in order to avoid race conditions and corrupt data.
### Examples
Start mavlink on ttyS1 serial with baudrate 921600 and maximum sending rate of 80kB/s:
$ mavlink start -d /dev/ttyS1 -b 921600 -m onboard -r 80000
Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz:
$ mavlink start -u 14556 -r 1000000
$ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink <command> [arguments...]
Commands:
start Start a new instance
[-d <val>] Select Serial Device
values: <file:dev>, default: /dev/ttyS1
[-b <val>] Baudrate (can also be p:<param_name>)
default: 57600
[-r <val>] Maximum sending data rate in B/s (if 0, use baudrate / 20)
default: 0
[-p] Enable Broadcast
[-u <val>] Select UDP Network Port (local)
default: 14556
[-o <val>] Select UDP Network Port (remote)
default: 14550
[-t <val>] Partner IP (broadcasting can be enabled via -p flag)
default: 127.0.0.1
[-m <val>] Mode: sets default streams and rates
values: custom|camera|onboard|osd|magic|config|iridium|minimal|
extvision|extvisionmin|gimbal, default: normal
[-n <val>] wifi/ethernet interface name
values: <interface_name>
[-c <val>] Multicast address (multicasting can be enabled via
MAV_i_BROADCAST param)
values: Multicast address in the range
[239.0.0.0,239.255.255.255]
[-f] Enable message forwarding to other Mavlink instances
[-w] Wait to send, until first message received
[-x] Enable FTP
[-z] Force hardware flow control always on
[-Z] Force hardware flow control always off
stop-all Stop all instances
stop Stop a running instance
[-u <val>] Select Mavlink instance via local Network Port
[-d <val>] Select Mavlink instance via Serial Device
values: <file:dev>
status Print status for all instances
[streams] Print all enabled streams
stream Configure the sending rate of a stream for a running instance
[-u <val>] Select Mavlink instance via local Network Port
[-d <val>] Select Mavlink instance via Serial Device
values: <file:dev>
-s <val> Mavlink stream to configure
-r <val> Rate in Hz (0 = turn off, -1 = set to default)
boot_complete Enable sending of messages. (Must be) called as last step in
startup script.
注：usage函数是具体对应实现。
class Mavlink final : public ModuleParams
2. 模块入口函数mavlink_main
模块除支持start/stop/status命令，自定义命令支持以下子命令：
- stop-all
- stream
- boot_complete
mavlink_main
├──> <argc < 2>
│ ├──> usage()
│ └──> return 1
├──> <!strcmp(argv[1], "start")> // 启动模块
│ └──> return Mavlink::start(argc, argv)
├──> <!strcmp(argv[1], "stop-all")> // 停止所有运行实例
│ └──> return Mavlink::destroy_all_instances()
├──> <!strcmp(argv[1], "status")> // 查询模块状态
│ ├──> bool show_streams_status = argc > 2 && strcmp(argv[2], "streams") == 0
│ └──> return Mavlink::get_status_all_instances(show_streams_status)
├──> <!strcmp(argv[1], "stop")> // 停止模块
│ └──> return Mavlink::stop_command(argc, argv)
├──> <!strcmp(argv[1], "stream")> // 配置管道实例参数
│ └──> return Mavlink::stream_command(argc, argv)
├──> <!strcmp(argv[1], "boot_complete")> //启动完成，标志可以开始发送消息
│ ├──> Mavlink::set_boot_complete()
│ └──> return 0
└──> <else>
├──> usage()
└──> return 1
3. mavlink模块重要函数
3.1 Mavlink::start
启动Mavlink实例。
Mavlink::start
├──> MavlinkULog::initialize()
├──> MavlinkCommandSender::initialize()
├──> [创建EventBuffer]
├──> [检查instance_count]
├──> px4_task_spawn_cmd("mavlink_main",
│ SCHED_DEFAULT,
│ SCHED_PRIORITY_DEFAULT,
│ PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
│ (px4_main_t)&Mavlink::start_helper,
│ (char *const *)argv);
└──> [检查instance_count]
Mavlink::start_helper
├──> Mavlink *instance = new Mavlink()
├──> <!instance>
│ └──> PX4_ERR("OUT OF MEM")
└──> <else>
└──> res = Mavlink::task_main
3.2 Mavlink::task_main
Mavlink业务服务
Mavlink::task_main
├──> [参数解析："b:r:d:n:u:o:m:t:c:fswxzZp"]
├──> [USB serial/SERIAL/UDP 端口&总线初始化]
├──> [Mavlink main loop之前的初始化]
├──> <!should_exit()>
│ ├──> [_parameter_update_sub.updated]
│ ├──> [_vehicle_status_sub.updated]
│ ├──> [_vehicle_command_sub.updated]
│ ├──> [_vehicle_command_ack_sub.updated]
│ ├──> [_gimbal_v1_command_sub.updated]
│ ├──> [check for shell output]
│ ├──> [update streams]
│ ├──> [check for ulog streaming messages]
│ ├──> [handle new events]
│ ├──> [pass messages from other UARTs // forwarding]
│ └──> [publish status at 1 Hz, or sooner if HEARTBEAT has updated]
└──> [Mavlink main loop之后的清理]
3.3 Mavlink::get_status_all_instances
查询每个实例的运行状态。
Mavlink::get_status_all_instances
├──> LockGuard lgmavlink_module_mutex
├──> unsigned iterations = 0
├──> <for (Mavlink *inst : mavlink_module_instances) >
│ └──> <inst != nullptr>
│ ├──> printf("\\ninstance #%u:\\n", iterations)
│ ├──> <show_streams_status> inst->display_status_streams()
│ ├──> <else> inst->display_status()
│ └──> iterations++
└──> return (iterations == 0) // return an error if there are no instances
3.4 Mavlink::stop_command
停止某一个Mavlink实例对象。
Mavlink::stop_command
├──> [解析命令参数: -u 本地网络端口; -d 串行设备]
├──> [寻找设备对象，并发起request_stop优雅退出过程]
└──> [优雅退出，并完成Mavlink清理工作: 内存释放等]
3.5 Mavlink::destroy_all_instances
通过request_stop方法通知运行实例进行优雅退出，最长等待1000 x 10000 us = 10 seconds。
Mavlink::destroy_all_instances
├──> PX4_INFO("waiting for instances to stop")
├──> [set flag to stop thread and wait for all threads to finish]
└──> [since all threads have exited, so it's safe to delete objects.]
3.6 Mavlink::set_boot_complete
设置标志位_boot_complete，表示系统启动完成，Mavlink消息通道打开可正常处理消息。
注：该标志位将会通过boot_complete成员方法在MavlinkReceiver::run调用，判断系统是否稳定就绪。
Mavlink::set_boot_complete
├──> _boot_complete = true // 类成员标记被置位true
└──> <MAVLINK_UDP>
├──> LockGuard lg mavlink_module_mutex
└──> <for (Mavlink *inst : mavlink_module_instances)>
├──> <inst && (inst->get_mode() != MAVLINK_MODE_ONBOARD) && !inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP>
└──> PX4_INFO("MAVLink only on localhost (set param MAV_i_BROADCAST = 1 to enable network)")
4. 总结
具体逻辑业务后续再做深入，从模块代码角度:
- 输入
uORB::Subscription _event_subORB_ID(event);
uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;
uORB::Subscription _vehicle_command_subORB_ID(vehicle_command);
uORB::Subscription _vehicle_command_ack_subORB_ID(vehicle_command_ack);
uORB::Subscription _vehicle_status_subORB_ID(vehicle_status);
uORB::Subscription _gimbal_v1_command_subORB_ID(gimbal_v1_command);
- 输出
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pubORB_ID(vehicle_command_ack);
uORB::PublicationMulti<telemetry_status_s> _telemetry_status_pubORB_ID(telemetry_status);
5. 参考资料
【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一：Built-In框架
【3】PX4模块设计之十二：High Resolution Timer设计
【4】PX4模块设计之十三：WorkQueue设计
【5】PX4模块设计之十七：ModuleBase模块
【6】PX4模块设计之三十：Hysteresis类
【7】PX4 modules_main
【8】PX4模块设计之四十一：I2C/SPI Bus Instance基础知识
以上是关于PX4模块设计之四十七：mavlink模块的主要内容，如果未能解决你的问题，请参考以下文章