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命令，自定义命令支持以下子命令：

1. stop-all
2. stream
3. 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 lg{mavlink_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_sub{ORB_ID(event)};
	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
	uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
	uORB::Subscription _gimbal_v1_command_sub{ORB_ID(gimbal_v1_command)};

• 输出

	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
	uORB::PublicationMulti<telemetry_status_s> _telemetry_status_pub{ORB_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基础知识