Mavlink_main.cpp源码学习

1. int mavlink_main(int argc, char *argv[])
2. {
3. if (argc < 2) {
4. usage();                                                             //使用说明
5. return 1;
6. }
7. if (!strcmp(argv[1], "start")) {                                         //在这里启动了mavlink
8. return Mavlink::start(argc, argv);
9. } else if (!strcmp(argv[1], "stop")) {                                   //命令是stop-all
10. PX4_WARN("mavlink stop is deprecated, use stop-all instead");
11. usage();
12. return 1;
13. } else if (!strcmp(argv[1], "stop-all")) {
14. return Mavlink::destroy_all_instances();                             //关闭start里创建的任务
15. } else if (!strcmp(argv[1], "status")) {
16. return Mavlink::get_status_all_instances();                        //打印start中创建的任务信息
17. } else if (!strcmp(argv[1], "stream")) {
18. return Mavlink::stream_command(argc, argv);
19. } else if (!strcmp(argv[1], "boot_complete")) {
20. Mavlink::set_boot_complete();
21. return 0;
22. } else {
23. usage();
24. return 1;
25. }
26. return 0;
27. }

1. int
2. Mavlink::start(int argc, char *argv[])
3. {
4. MavlinkULog::initialize();
5. MavlinkCommandSender::initialize();
6. // Wait for the instance count to go up one
7. // before returning to the shell
8. int ic = Mavlink::instance_count();                                     //确认不超过最大个数
9. if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
10. warnx("Maximum MAVLink instance count of %d reached.",
11. (int)Mavlink::MAVLINK_MAX_INSTANCES);
12. return 1;
13. }
14. // Instantiate thread
15. char buf[24];
16. sprintf(buf, "mavlink_if%d", ic);
17. // This is where the control flow splits
18. // between the starting task and the spawned
19. // task - start_helper() only returns
20. // when the started task exits.                                     //这里创建了start_helper的守护进程
21. px4_task_spawn_cmd(buf,
22. SCHED_DEFAULT,
23. SCHED_PRIORITY_DEFAULT,
24. 2500,
25. (px4_main_t)&Mavlink::start_helper,
26. (char *const *)argv);
27. // Ensure that this shell command
28. // does not return before the instance
29. // is fully initialized. As this is also
30. // the only path to create a new instance,
31. // this is effectively a lock on concurrent
32. // instance starting. XXX do a real lock.
33. // Sleep 500 us between each attempt
34. const unsigned sleeptime = 500;
35. // Wait 100 ms max for the startup.
36. const unsigned limit = 100 * 1000 / sleeptime;
37. unsigned count = 0;
38. while (ic == Mavlink::instance_count() && count < limit) {                     //轮询等待
39. ::usleep(sleeptime);
40. count++;
41. }
42. return OK;
43. }

1. int
2. Mavlink::task_main(int argc, char *argv[])
3. {
4. int ch;
5. _baudrate = 57600;
6. _datarate = 0;
7. _mode = MAVLINK_MODE_NORMAL;
8. #ifdef __PX4_NUTTX
9. /* the NuttX optarg handler does not
10. * ignore argv[0] like the POSIX handler
11. * does, nor does it deal with non-flag
12. * verbs well. So we remove the application
13. * name and the verb.
14. */
15. argc -= 2;
16. argv += 2;
17. #endif
18. /* don't exit from getopt loop to leave getopt global variables in consistent state,
19. * set error flag instead */
20. bool err_flag = false;
21. int myoptind = 1;
22. const char *myoptarg = nullptr;
23. #ifdef __PX4_POSIX
24. char *eptr;
25. int temp_int_arg;
26. #endif
27. while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fvwx", &myoptind, &myoptarg)) != EOF) {            //这里把argv[0]后面的都反了一下
28. switch (ch) {
29. case 'b':
30. _baudrate = strtoul(myoptarg, nullptr, 10);                             //设置波特率
31. if (_baudrate < 9600 || _baudrate > 3000000) {
32. warnx("invalid baud rate '%s'", myoptarg);
33. err_flag = true;
34. }
35. break;
36. case 'r':
37. _datarate = strtoul(myoptarg, nullptr, 10);                    //设置传输速率
38. if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
39. warnx("invalid data rate '%s'", myoptarg);
40. err_flag = true;
41. }
42. break;
43. case 'd':
44. _device_name = myoptarg;                                      //选择串口设备
45. set_protocol(SERIAL);
46. break;
47. #ifdef __PX4_POSIX
48. case 'u':                                                            //选择UDP网络本地
49. temp_int_arg = strtoul(myoptarg, &eptr, 10);
50. if (*eptr == '\0') {
51. _network_port = temp_int_arg;
52. set_protocol(UDP);
53. } else {
54. warnx("invalid data udp_port '%s'", myoptarg);
55. err_flag = true;
56. }
57. break;
58. case 'o':                                                        //选择UDP网络远程
59. temp_int_arg = strtoul(myoptarg, &eptr, 10);
60. if (*eptr == '\0') {
61. _remote_port = temp_int_arg;
62. set_protocol(UDP);
63. } else {
64. warnx("invalid remote udp_port '%s'", myoptarg);
65. err_flag = true;
66. }
67. break;
68. case 't':                                                //伙伴IP，通过MAV_BROADCAST参数广播
69. _src_addr.sin_family = AF_INET;
70. if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
71. _src_addr_initialized = true;
72. } else {
73. warnx("invalid partner ip '%s'", myoptarg);
74. err_flag = true;
75. }
76. break;
77. #else
78. case 'u':
79. case 'o':
80. case 't':
81. warnx("UDP options not supported on this platform");
82. err_flag = true;
83. break;
84. #endif
85. //        case 'e':
86. //            mavlink_link_termination_allowed = true;
87. //            break;
88. case 'm':                                                    //设置默认的stream和rates
89. if (strcmp(myoptarg, "custom") == 0) {
90. _mode = MAVLINK_MODE_CUSTOM;
91. } else if (strcmp(myoptarg, "camera") == 0) {
92. // left in here for compatibility
93. _mode = MAVLINK_MODE_ONBOARD;
94. } else if (strcmp(myoptarg, "onboard") == 0) {
95. _mode = MAVLINK_MODE_ONBOARD;
96. } else if (strcmp(myoptarg, "osd") == 0) {
97. _mode = MAVLINK_MODE_OSD;
98. } else if (strcmp(myoptarg, "magic") == 0) {
99. _mode = MAVLINK_MODE_MAGIC;
100. } else if (strcmp(myoptarg, "config") == 0) {
101. _mode = MAVLINK_MODE_CONFIG;
102. } else if (strcmp(myoptarg, "iridium") == 0) {
103. _mode = MAVLINK_MODE_IRIDIUM;
104. _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
105. }
106. break;
107. case 'f':                                                        //消息可发给其他mavlink接口
108. _forwarding_on = true;
109. break;
110. case 'v':                                                        //verbos output
111. _verbose = true;
112. break;
113. case 'w':                                                        //等待发送直到接收到第一个消息
114. _wait_to_transmit = true;
115. break;
116. case 'x':                                                        //开启ftp
117. _ftp_on = true;
118. break;
119. default:                                                        //其他应该是错误的
120. err_flag = true;
121. break;
122. }
123. }
124. if (err_flag) {
125. usage();
126. return PX4_ERROR;
127. }
128. if (_datarate == 0) {
129. /* convert bits to bytes and use 1/2 of bandwidth by default */
130. _datarate = _baudrate / 20;
131. }
132. if (_datarate > MAX_DATA_RATE) {
133. _datarate = MAX_DATA_RATE;
134. }
135. if (get_protocol() == SERIAL) {
136. if (Mavlink::instance_exists(_device_name, this)) {
137. warnx("%s already running", _device_name);
138. return PX4_ERROR;
139. }
140. PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
141. mavlink_mode_str(_mode), _datarate, _device_name, _baudrate);
142. /* flush stdout in case MAVLink is about to take it over */                                //清空输出缓冲区，并把缓冲区内容输出
143. fflush(stdout);
144. /* default values for arguments */                                                        //默认的串口值
145. _uart_fd = mavlink_open_uart(_baudrate, _device_name);
146. if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
147. warn("could not open %s", _device_name);
148. return PX4_ERROR;
149. } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
150. /* the config link is optional */
151. return OK;
152. }
153. } else if (get_protocol() == UDP) {
154. if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
155. warnx("port %d already occupied", _network_port);
156. return PX4_ERROR;
157. }
158. PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
159. mavlink_mode_str(_mode), _datarate, _network_port, _remote_port);
160. }
161. /* initialize send mutex */                                                        //初始化发送mutex
162. pthread_mutex_init(&_send_mutex, nullptr);
163. /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */                //如果我们在传送mavlink消息，我们需要准备一个buffer作为接口
164. if (_forwarding_on || _ftp_on) {
165. /* initialize message buffer if multiplexing is on or its needed for FTP.                             //如果multiplexing打开了或者需要FTP，初始化消息buffer。
166. * make space for two messages plus off-by-one space as we use the empty element                      //为两个消息提供足够的空间，并加上off-by-one空间，作为我们使用的空元素
167. * marker ring buffer approach.                                                                       //标记ring buffer方法
168. */
169. if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {                                   //起始申请了3个message空间，第三个应该是空的。防止off-by-one
170. warnx("msg buf:");
171. return 1;
172. }
173. /* initialize message buffer mutex */                                                                 //初始化message buffer mutex
174. pthread_mutex_init(&_message_buffer_mutex, nullptr);
175. }
176. /* Initialize system properties */                                                                        //初始化系统内容
177. mavlink_update_system();
178. /* start the MAVLink receiver */                                                                            //开始接收mavlink
179. MavlinkReceiver::receive_start(&_receive_thread, this);
180. MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
181. uint64_t param_time = 0;
182. MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
183. uint64_t status_time = 0;
184. MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
185. /* We don't want to miss the first advertise of an ACK, so we subscribe from the                                //我们不想要丢失最开始ACK的advertise，所以我们topic退出后，还会发送一段时间
186. * beginning and not just when the topic exists. */
187. ack_sub->subscribe_from_beginning(true);
188. uint64_t ack_time = 0;
189. MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
190. struct vehicle_status_s status;
191. status_sub->update(&status_time, &status);
192. struct vehicle_command_ack_s command_ack;
193. ack_sub->update(&ack_time, &command_ack);
194. /* add default streams depending on mode */                                                                            //根据mode,增加默认的streams
195. if (_mode != MAVLINK_MODE_IRIDIUM) {
196. /* HEARTBEAT is constant rate stream, rate never adjusted */
197. configure_stream("HEARTBEAT", 1.0f);
198. /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
199. configure_stream("STATUSTEXT", 20.0f);
200. /* COMMAND_LONG stream: use high rate to avoid commands skipping */
201. configure_stream("COMMAND_LONG", 100.0f);
202. }
203. switch (_mode) {
204. case MAVLINK_MODE_NORMAL:
205. configure_stream("SYS_STATUS", 1.0f);
206. configure_stream("EXTENDED_SYS_STATE", 1.0f);
207. configure_stream("HIGHRES_IMU", 1.5f);
208. configure_stream("ATTITUDE", 20.0f);
209. configure_stream("RC_CHANNELS", 5.0f);
210. configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
211. configure_stream("ALTITUDE", 1.0f);
212. configure_stream("GPS_RAW_INT", 1.0f);
213. configure_stream("ADSB_VEHICLE", 2.0f);
214. configure_stream("COLLISION", 2.0f);
215. configure_stream("DISTANCE_SENSOR", 0.5f);
216. configure_stream("OPTICAL_FLOW_RAD", 1.0f);
217. configure_stream("VISION_POSITION_ESTIMATE", 1.0f);
218. configure_stream("ESTIMATOR_STATUS", 0.5f);
219. configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
220. configure_stream("GLOBAL_POSITION_INT", 5.0f);
221. configure_stream("LOCAL_POSITION_NED", 1.0f);
222. configure_stream("POSITION_TARGET_LOCAL_NED", 1.5f);
223. configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
224. configure_stream("ATTITUDE_TARGET", 2.0f);
225. configure_stream("HOME_POSITION", 0.5f);
226. configure_stream("NAMED_VALUE_FLOAT", 1.0f);
227. configure_stream("VFR_HUD", 4.0f);
228. configure_stream("WIND_COV", 1.0f);
229. break;
230. case MAVLINK_MODE_ONBOARD:
231. configure_stream("SYS_STATUS", 5.0f);
232. configure_stream("EXTENDED_SYS_STATE", 5.0f);
233. configure_stream("HIGHRES_IMU", 50.0f);
234. configure_stream("ATTITUDE", 250.0f);
235. configure_stream("ATTITUDE_QUATERNION", 50.0f);
236. configure_stream("RC_CHANNELS", 20.0f);
237. configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
238. configure_stream("ALTITUDE", 10.0f);
239. configure_stream("GPS_RAW_INT", 5.0f);
240. configure_stream("ADSB_VEHICLE", 10.0f);
241. configure_stream("COLLISION", 10.0f);
242. configure_stream("DISTANCE_SENSOR", 10.0f);
243. configure_stream("OPTICAL_FLOW_RAD", 10.0f);
244. configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
245. configure_stream("ESTIMATOR_STATUS", 1.0f);
246. configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
247. configure_stream("GLOBAL_POSITION_INT", 50.0f);
248. configure_stream("LOCAL_POSITION_NED", 30.0f);
249. configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
250. configure_stream("ATTITUDE_TARGET", 10.0f);
251. configure_stream("HOME_POSITION", 0.5f);
252. configure_stream("NAMED_VALUE_FLOAT", 10.0f);
253. configure_stream("VFR_HUD", 10.0f);
254. configure_stream("WIND_COV", 10.0f);
255. configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
256. configure_stream("SYSTEM_TIME", 1.0f);
257. configure_stream("TIMESYNC", 10.0f);
258. configure_stream("CAMERA_CAPTURE", 2.0f);
259. //camera trigger is rate limited at the source, do not limit here
260. configure_stream("CAMERA_TRIGGER", 500.0f);
261. configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
262. configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
263. break;
264. case MAVLINK_MODE_OSD:
265. configure_stream("SYS_STATUS", 5.0f);
266. configure_stream("EXTENDED_SYS_STATE", 1.0f);
267. configure_stream("ATTITUDE", 25.0f);
268. configure_stream("RC_CHANNELS", 5.0f);
269. configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
270. configure_stream("ALTITUDE", 1.0f);
271. configure_stream("GPS_RAW_INT", 1.0f);
272. configure_stream("ESTIMATOR_STATUS", 1.0f);
273. configure_stream("GLOBAL_POSITION_INT", 10.0f);
274. configure_stream("ATTITUDE_TARGET", 10.0f);
275. configure_stream("HOME_POSITION", 0.5f);
276. configure_stream("VFR_HUD", 25.0f);
277. configure_stream("WIND_COV", 2.0f);
278. configure_stream("SYSTEM_TIME", 1.0f);
279. break;
280. case MAVLINK_MODE_MAGIC:
281. //stream nothing
282. break;
283. case MAVLINK_MODE_CONFIG:
284. // Enable a number of interesting streams we want via USB
285. configure_stream("SYS_STATUS", 1.0f);
286. configure_stream("EXTENDED_SYS_STATE", 2.0f);
287. configure_stream("HIGHRES_IMU", 50.0f);
288. configure_stream("ATTITUDE", 50.0f);
289. configure_stream("ATTITUDE_QUATERNION", 50.0f);
290. configure_stream("RC_CHANNELS", 10.0f);
291. configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
292. configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
293. configure_stream("ALTITUDE", 10.0f);
294. configure_stream("GPS_RAW_INT", 10.0f);
295. configure_stream("ADSB_VEHICLE", 20.0f);
296. configure_stream("COLLISION", 20.0f);
297. configure_stream("DISTANCE_SENSOR", 10.0f);
298. configure_stream("OPTICAL_FLOW_RAD", 10.0f);
299. configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
300. configure_stream("ESTIMATOR_STATUS", 5.0f);
301. configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
302. configure_stream("GLOBAL_POSITION_INT", 10.0f);
303. configure_stream("LOCAL_POSITION_NED", 30.0f);
304. configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
305. configure_stream("ATTITUDE_TARGET", 8.0f);
306. configure_stream("HOME_POSITION", 0.5f);
307. configure_stream("NAMED_VALUE_FLOAT", 50.0f);
308. configure_stream("VFR_HUD", 20.0f);
309. configure_stream("WIND_COV", 10.0f);
310. configure_stream("CAMERA_TRIGGER", 500.0f);
311. configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
312. configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
313. configure_stream("MANUAL_CONTROL", 5.0f);
314. break;
315. case MAVLINK_MODE_IRIDIUM:
316. configure_stream("HIGH_LATENCY", 0.1f);
317. break;
318. default:
319. break;
320. }
321. /* set main loop delay depending on data rate to minimize CPU overhead */                                            //根据data reate减少CPU开销,设置主要loop延迟
322. _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
323. /* hard limit to 500 Hz at max */                                                                                    //硬界限到500Hz最大
324. if (_main_loop_delay < 2000) {
325. _main_loop_delay = 2000;
326. }
327. /* hard limit to 100 Hz at least */                                                                                  //最小100Hz
328. if (_main_loop_delay > 10000) {
329. _main_loop_delay = 10000;
330. }
331. /* now the instance is fully initialized and we can bump the instance count */                                       //现在实例完全初始化，并且我们能bump实例计数
332. LL_APPEND(_mavlink_instances, this);
333. /* init socket if necessary */                                                                                        //如果需要初始化socket
334. if (get_protocol() == UDP) {
335. init_udp();
336. }
337. /* if the protocol is serial, we send the system version blindly */                                                    //如果协议是攒口，发送系统版本blindy(盲目的）
338. if (get_protocol() == SERIAL) {
339. send_autopilot_capabilites();
340. }
341. while (!_task_should_exit) {
342. /* main loop */                                                                                                //主循环
343. usleep(_main_loop_delay);
344. perf_begin(_loop_perf);
345. hrt_abstime t = hrt_absolute_time();
346. update_rate_mult();
347. if (param_sub->update(&param_time, nullptr)) {
348. /* parameters updated */                                                                                //参数更新
349. mavlink_update_system();
350. }
351. /* radio config check */                                                                                    //比例设置确认
352. if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
353. /* request to configure radio and radio is present */
354. FILE *fs = fdopen(_uart_fd, "w");
355. if (fs) {
356. /* switch to AT command mode */                                                                    //转换到AT命令模式
357. usleep(1200000);
358. fprintf(fs, "+++\n");
359. usleep(1200000);
360. if (_radio_id > 0) {
361. /* set channel */
362. fprintf(fs, "ATS3=%u\n", _radio_id);
363. usleep(200000);
364. } else {
365. /* reset to factory defaults */                                                //重置factory defaults
366. fprintf(fs, "AT&F\n");
367. usleep(200000);
368. }
369. /* write config */                                                                //写配置
370. fprintf(fs, "AT&W");
371. usleep(200000);
372. /* reboot */                                                                        //重启
373. fprintf(fs, "ATZ");
374. usleep(200000);
375. // XXX NuttX suffers from a bug where
376. // fclose() also closes the fd, not just
377. // the file stream. Since this is a one-time
378. // config thing, we leave the file struct
379. // allocated.
380. #ifndef __PX4_NUTTX
381. fclose(fs);
382. #endif
383. } else {
384. PX4_WARN("open fd %d failed", _uart_fd);
385. }
386. /* reset param and save */                                                                    //重设param并且保存
387. _radio_id = 0;
388. param_set(_param_radio_id, &_radio_id);
389. }
390. if (status_sub->update(&status_time, &status)) {
391. /* switch HIL mode if required */                                                             //转换HIL模式如果需要
392. set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
393. set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
394. }
395. /* send command ACK */                                                                            //发送ACK命令
396. uint16_t current_command_ack = 0;
397. if (ack_sub->update(&ack_time, &command_ack)) {
398. mavlink_command_ack_t msg;
399. msg.result = command_ack.result;
400. msg.command = command_ack.command;
401. current_command_ack = command_ack.command;
402. mavlink_msg_command_ack_send_struct(get_channel(), &msg);
403. }
404. struct mavlink_log_s mavlink_log;
405. if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
406. _logbuffer.put(&mavlink_log);
407. }
408. /* check for shell output */                                                                                    //确认作为shell输出
409. if (_mavlink_shell && _mavlink_shell->available() > 0) {
410. if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
411. mavlink_serial_control_t msg;
412. msg.baudrate = 0;
413. msg.flags = SERIAL_CONTROL_FLAG_REPLY;
414. msg.timeout = 0;
415. msg.device = SERIAL_CONTROL_DEV_SHELL;
416. msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
417. mavlink_msg_serial_control_send_struct(get_channel(), &msg);
418. }
419. }
420. /* check for ulog streaming messages */                                                                        //确认作为ulog streaming消息
421. if (_mavlink_ulog) {
422. if (_mavlink_ulog_stop_requested) {
423. _mavlink_ulog->stop();
424. _mavlink_ulog = nullptr;
425. _mavlink_ulog_stop_requested = false;
426. } else {
427. if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
428. _mavlink_ulog->start_ack_received();
429. }
430. int ret = _mavlink_ulog->handle_update(get_channel());
431. if (ret < 0) { //abort the streaming on error
432. if (ret != -1) {
433. PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
434. }
435. _mavlink_ulog->stop();
436. _mavlink_ulog = nullptr;
437. }
438. }
439. }
440. /* check for requested subscriptions */                                                                    //确认requested subscriptions
441. if (_subscribe_to_stream != nullptr) {
442. if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
443. if (_subscribe_to_stream_rate > 0.0f) {
444. if (get_protocol() == SERIAL) {
445. PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
446. (double)_subscribe_to_stream_rate);
447. } else if (get_protocol() == UDP) {
448. PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
449. (double)_subscribe_to_stream_rate);
450. }
451. } else {
452. if (get_protocol() == SERIAL) {
453. PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
454. } else if (get_protocol() == UDP) {
455. PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
456. }
457. }
458. } else {
459. if (get_protocol() == SERIAL) {
460. PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);
461. } else if (get_protocol() == UDP) {
462. PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
463. }
464. }
465. _subscribe_to_stream = nullptr;
466. }
467. /* update streams */                                                                                                     //更新流
468. MavlinkStream *stream;
469. LL_FOREACH(_streams, stream) {
470. stream->update(t);
471. }
472. /* pass messages from other UARTs or FTP worker */                                                                    //传递消息从其他UARTs或FTP 队列
473. if (_forwarding_on || _ftp_on) {
474. bool is_part;
475. uint8_t *read_ptr;
476. uint8_t *write_ptr;
477. pthread_mutex_lock(&_message_buffer_mutex);
478. int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
479. pthread_mutex_unlock(&_message_buffer_mutex);
480. if (available > 0) {
481. // Reconstruct message from buffer
482. mavlink_message_t msg;
483. write_ptr = (uint8_t *)&msg;
484. // Pull a single message from the buffer
485. size_t read_count = available;
486. if (read_count > sizeof(mavlink_message_t)) {
487. read_count = sizeof(mavlink_message_t);
488. }
489. memcpy(write_ptr, read_ptr, read_count);
490. // We hold the mutex until after we complete the second part of the buffer. If we don