Mavlink_main.cpp源码学习


  1. int mavlink_main(int argc, char *argv[])
  2. {
  3.     if (argc < 2) {
  4.         usage();                                                             //使用说明
  5.         return 1;
  6.     }

  8.     if (!strcmp(argv[1], "start")) {                                         //在这里启动了mavlink   
  9.         return Mavlink::start(argc, argv);                

  11.     } else if (!strcmp(argv[1], "stop")) {                                   //命令是stop-all
  12.         PX4_WARN("mavlink stop is deprecated, use stop-all instead");
  13.         usage();
  14.         return 1;

  16.     } else if (!strcmp(argv[1], "stop-all")) {
  17.         return Mavlink::destroy_all_instances();                             //关闭start里创建的任务

  19.     } else if (!strcmp(argv[1], "status")) {
  20.         return Mavlink::get_status_all_instances();                        //打印start中创建的任务信息

  22.     } else if (!strcmp(argv[1], "stream")) {
  23.         return Mavlink::stream_command(argc, argv);

  25.     } else if (!strcmp(argv[1], "boot_complete")) {
  26.         Mavlink::set_boot_complete();
  27.         return 0;

  29.     } else {
  30.         usage();
  31.         return 1;
  32.     }

  34.     return 0;
  35. }

  1. int
  2. Mavlink::start(int argc, char *argv[])
  3. {
  4.     MavlinkULog::initialize();
  5.     MavlinkCommandSender::initialize();

  7.     // Wait for the instance count to go up one
  8.     // before returning to the shell
  9.     int ic = Mavlink::instance_count();                                     //确认不超过最大个数

  11.     if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
  12.         warnx("Maximum MAVLink instance count of %d reached.",
  13.          (int)Mavlink::MAVLINK_MAX_INSTANCES);
  14.         return 1;
  15.     }

  17.     // Instantiate thread
  18.     char buf[24];
  19.     sprintf(buf, "mavlink_if%d", ic);

  21.     // This is where the control flow splits
  22.     // between the starting task and the spawned
  23.     // task - start_helper() only returns
  24.     // when the started task exits.                                     //这里创建了start_helper的守护进程
  25.     px4_task_spawn_cmd(buf,
  26.              SCHED_DEFAULT,
  27.              SCHED_PRIORITY_DEFAULT,
  28.              2500,
  29.              (px4_main_t)&Mavlink::start_helper,
  30.              (char *const *)argv);

  32.     // Ensure that this shell command
  33.     // does not return before the instance
  34.     // is fully initialized. As this is also
  35.     // the only path to create a new instance,
  36.     // this is effectively a lock on concurrent
  37.     // instance starting. XXX do a real lock.

  39.     // Sleep 500 us between each attempt
  40.     const unsigned sleeptime = 500;

  42.     // Wait 100 ms max for the startup.
  43.     const unsigned limit = 100 * 1000 / sleeptime;

  45.     unsigned count = 0;

  47.     while (ic == Mavlink::instance_count() && count < limit) {                     //轮询等待
  48.         ::usleep(sleeptime);
  49.         count++;
  50.     }

  52.     return OK;
  53. }



  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;

  9. #ifdef __PX4_NUTTX
  10.     /* the NuttX optarg handler does not
  11.      * ignore argv[0] like the POSIX handler
  12.      * does, nor does it deal with non-flag
  13.      * verbs well. So we remove the application
  14.      * name and the verb.
  15.      */
  16.     argc -= 2;
  17.     argv += 2;
  18. #endif

  20.     /* don't exit from getopt loop to leave getopt global variables in consistent state,
  21.      * set error flag instead */
  22.     bool err_flag = false;
  23.     int myoptind = 1;
  24.     const char *myoptarg = nullptr;
  25. #ifdef __PX4_POSIX
  26.     char *eptr;
  27.     int temp_int_arg;
  28. #endif

  30.     while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fvwx", &myoptind, &myoptarg)) != EOF) {            //这里把argv[0]后面的都反了一下
  31.         switch (ch) {
  32.         case 'b':         
  33.             _baudrate = strtoul(myoptarg, nullptr, 10);                             //设置波特率

  35.             if (_baudrate < 9600 || _baudrate > 3000000) {
  36.                 warnx("invalid baud rate '%s'", myoptarg);
  37.                 err_flag = true;
  38.             }

  40.             break;

  42.         case 'r':
  43.             _datarate = strtoul(myoptarg, nullptr, 10);                    //设置传输速率

  45.             if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
  46.                 warnx("invalid data rate '%s'", myoptarg);
  47.                 err_flag = true;
  48.             }

  50.             break;

  52.         case 'd':
  53.             _device_name = myoptarg;                                      //选择串口设备
  54.             set_protocol(SERIAL);
  55.             break;

  57. #ifdef __PX4_POSIX

  59.         case 'u':                                                            //选择UDP网络本地
  60.             temp_int_arg = strtoul(myoptarg, &eptr, 10);

  62.             if (*eptr == '0') {
  63.                 _network_port = temp_int_arg;
  64.                 set_protocol(UDP);

  66.             } else {
  67.                 warnx("invalid data udp_port '%s'", myoptarg);
  68.                 err_flag = true;
  69.             }

  71.             break;

  73.         case 'o':                                                        //选择UDP网络远程
  74.             temp_int_arg = strtoul(myoptarg, &eptr, 10);

  76.             if (*eptr == '0') {
  77.                 _remote_port = temp_int_arg;
  78.                 set_protocol(UDP);

  80.             } else {
  81.                 warnx("invalid remote udp_port '%s'", myoptarg);
  82.                 err_flag = true;
  83.             }

  85.             break;

  87.         case 't':                                                //伙伴IP，通过MAV_BROADCAST参数广播
  88.             _src_addr.sin_family = AF_INET;

  90.             if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
  91.                 _src_addr_initialized = true;

  93.             } else {
  94.                 warnx("invalid partner ip '%s'", myoptarg);
  95.                 err_flag = true;
  96.             }

  98.             break;
  99. #else

  101.         case 'u':
  102.         case 'o':
  103.         case 't':
  104.             warnx("UDP options not supported on this platform");
  105.             err_flag = true;
  106.             break;
  107. #endif

  109. //        case 'e':
  110. //            mavlink_link_termination_allowed = true;
  111. //            break;

  113.         case 'm':                                                    //设置默认的stream和rates
  114.             if (strcmp(myoptarg, "custom") == 0) {
  115.                 _mode = MAVLINK_MODE_CUSTOM;

  117.             } else if (strcmp(myoptarg, "camera") == 0) {
  118.                 // left in here for compatibility
  119.                 _mode = MAVLINK_MODE_ONBOARD;

  121.             } else if (strcmp(myoptarg, "onboard") == 0) {
  122.                 _mode = MAVLINK_MODE_ONBOARD;

  124.             } else if (strcmp(myoptarg, "osd") == 0) {
  125.                 _mode = MAVLINK_MODE_OSD;

  127.             } else if (strcmp(myoptarg, "magic") == 0) {
  128.                 _mode = MAVLINK_MODE_MAGIC;

  130.             } else if (strcmp(myoptarg, "config") == 0) {
  131.                 _mode = MAVLINK_MODE_CONFIG;

  133.             } else if (strcmp(myoptarg, "iridium") == 0) {
  134.                 _mode = MAVLINK_MODE_IRIDIUM;
  135.                 _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
  136.             }

  138.             break;

  140.         case 'f':                                                        //消息可发给其他mavlink接口
  141.             _forwarding_on = true;
  142.             break;

  144.         case 'v':                                                        //verbos output
  145.             _verbose = true;
  146.             break;

  148.         case 'w':                                                        //等待发送直到接收到第一个消息
  149.             _wait_to_transmit = true;
  150.             break;

  152.         case 'x':                                                        //开启ftp
  153.             _ftp_on = true;
  154.             break;

  156.         default:                                                        //其他应该是错误的
  157.             err_flag = true;
  158.             break;
  159.         }
  160.     }

  162.     if (err_flag) {
  163.         usage();
  164.         return PX4_ERROR;
  165.     }

  167.     if (_datarate == 0) {
  168.         /* convert bits to bytes and use 1/2 of bandwidth by default */
  169.         _datarate = _baudrate / 20;
  170.     }

  172.     if (_datarate > MAX_DATA_RATE) {
  173.         _datarate = MAX_DATA_RATE;
  174.     }

  176.     if (get_protocol() == SERIAL) {
  177.         if (Mavlink::instance_exists(_device_name, this)) {
  178.             warnx("%s already running", _device_name);
  179.             return PX4_ERROR;
  180.         }

  182.         PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
  183.              mavlink_mode_str(_mode), _datarate, _device_name, _baudrate);

  185.         /* flush stdout in case MAVLink is about to take it over */                                //清空输出缓冲区，并把缓冲区内容输出
  186.         fflush(stdout);

  188.         /* default values for arguments */                                                        //默认的串口值
  189.         _uart_fd = mavlink_open_uart(_baudrate, _device_name);

  191.         if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
  192.             warn("could not open %s", _device_name);
  193.             return PX4_ERROR;

  195.         } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
  196.             /* the config link is optional */
  197.             return OK;
  198.         }

  200.     } else if (get_protocol() == UDP) {
  201.         if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
  202.             warnx("port %d already occupied", _network_port);
  203.             return PX4_ERROR;
  204.         }

  206.         PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
  207.              mavlink_mode_str(_mode), _datarate, _network_port, _remote_port);
  208.     }

  210.     /* initialize send mutex */                                                        //初始化发送mutex
  211.     pthread_mutex_init(&_send_mutex, nullptr);

  213.     /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */                //如果我们在传送mavlink消息，我们需要准备一个buffer作为接口
  214.     if (_forwarding_on || _ftp_on) {
  215.         /* initialize message buffer if multiplexing is on or its needed for FTP.                             //如果multiplexing打开了或者需要FTP，初始化消息buffer。
  216.          * make space for two messages plus off-by-one space as we use the empty element                      //为两个消息提供足够的空间，并加上off-by-one空间，作为我们使用的空元素
  217.          * marker ring buffer approach.                                                                       //标记ring buffer方法
  218.          */
  219.         if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {                                   //起始申请了3个message空间，第三个应该是空的。防止off-by-one
  220.             warnx("msg buf:");
  221.             return 1;
  222.         }

  224.         /* initialize message buffer mutex */                                                                 //初始化message buffer mutex
  225.         pthread_mutex_init(&_message_buffer_mutex, nullptr);
  226.     }

  228.     /* Initialize system properties */                                                                        //初始化系统内容
  229.     mavlink_update_system();

  231.     /* start the MAVLink receiver */                                                                            //开始接收mavlink
  232.     MavlinkReceiver::receive_start(&_receive_thread, this);

  234.     MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
  235.     uint64_t param_time = 0;
  236.     MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
  237.     uint64_t status_time = 0;
  238.     MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
  239.     /* We don't want to miss the first advertise of an ACK, so we subscribe from the                                //我们不想要丢失最开始ACK的advertise，所以我们topic退出后，还会发送一段时间
  240.      * beginning and not just when the topic exists. */
  241.     ack_sub->subscribe_from_beginning(true);

  243.     uint64_t ack_time = 0;
  244.     MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));

  246.     struct vehicle_status_s status;
  247.     status_sub->update(&status_time, &status);
  248.     struct vehicle_command_ack_s command_ack;
  249.     ack_sub->update(&ack_time, &command_ack);

  251.     /* add default streams depending on mode */                                                                            //根据mode,增加默认的streams

  253.     if (_mode != MAVLINK_MODE_IRIDIUM) {

  255.         /* HEARTBEAT is constant rate stream, rate never adjusted */
  256.         configure_stream("HEARTBEAT", 1.0f);

  258.         /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
  259.         configure_stream("STATUSTEXT", 20.0f);

  261.         /* COMMAND_LONG stream: use high rate to avoid commands skipping */
  262.         configure_stream("COMMAND_LONG", 100.0f);

  264.     }

  266.     switch (_mode) {
  267.     case MAVLINK_MODE_NORMAL:
  268.         configure_stream("SYS_STATUS", 1.0f);
  269.         configure_stream("EXTENDED_SYS_STATE", 1.0f);
  270.         configure_stream("HIGHRES_IMU", 1.5f);
  271.         configure_stream("ATTITUDE", 20.0f);
  272.         configure_stream("RC_CHANNELS", 5.0f);
  273.         configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
  274.         configure_stream("ALTITUDE", 1.0f);
  275.         configure_stream("GPS_RAW_INT", 1.0f);
  276.         configure_stream("ADSB_VEHICLE", 2.0f);
  277.         configure_stream("COLLISION", 2.0f);
  278.         configure_stream("DISTANCE_SENSOR", 0.5f);
  279.         configure_stream("OPTICAL_FLOW_RAD", 1.0f);
  280.         configure_stream("VISION_POSITION_ESTIMATE", 1.0f);
  281.         configure_stream("ESTIMATOR_STATUS", 0.5f);
  282.         configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
  283.         configure_stream("GLOBAL_POSITION_INT", 5.0f);
  284.         configure_stream("LOCAL_POSITION_NED", 1.0f);
  285.         configure_stream("POSITION_TARGET_LOCAL_NED", 1.5f);
  286.         configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
  287.         configure_stream("ATTITUDE_TARGET", 2.0f);
  288.         configure_stream("HOME_POSITION", 0.5f);
  289.         configure_stream("NAMED_VALUE_FLOAT", 1.0f);
  290.         configure_stream("VFR_HUD", 4.0f);
  291.         configure_stream("WIND_COV", 1.0f);
  292.         break;

  294.     case MAVLINK_MODE_ONBOARD:
  295.         configure_stream("SYS_STATUS", 5.0f);
  296.         configure_stream("EXTENDED_SYS_STATE", 5.0f);
  297.         configure_stream("HIGHRES_IMU", 50.0f);
  298.         configure_stream("ATTITUDE", 250.0f);
  299.         configure_stream("ATTITUDE_QUATERNION", 50.0f);
  300.         configure_stream("RC_CHANNELS", 20.0f);
  301.         configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
  302.         configure_stream("ALTITUDE", 10.0f);
  303.         configure_stream("GPS_RAW_INT", 5.0f);
  304.         configure_stream("ADSB_VEHICLE", 10.0f);
  305.         configure_stream("COLLISION", 10.0f);
  306.         configure_stream("DISTANCE_SENSOR", 10.0f);
  307.         configure_stream("OPTICAL_FLOW_RAD", 10.0f);
  308.         configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
  309.         configure_stream("ESTIMATOR_STATUS", 1.0f);
  310.         configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
  311.         configure_stream("GLOBAL_POSITION_INT", 50.0f);
  312.         configure_stream("LOCAL_POSITION_NED", 30.0f);
  313.         configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
  314.         configure_stream("ATTITUDE_TARGET", 10.0f);
  315.         configure_stream("HOME_POSITION", 0.5f);
  316.         configure_stream("NAMED_VALUE_FLOAT", 10.0f);
  317.         configure_stream("VFR_HUD", 10.0f);
  318.         configure_stream("WIND_COV", 10.0f);
  319.         configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
  320.         configure_stream("SYSTEM_TIME", 1.0f);
  321.         configure_stream("TIMESYNC", 10.0f);
  322.         configure_stream("CAMERA_CAPTURE", 2.0f);
  323.         //camera trigger is rate limited at the source, do not limit here
  324.         configure_stream("CAMERA_TRIGGER", 500.0f);
  325.         configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
  326.         configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
  327.         break;

  329.     case MAVLINK_MODE_OSD:
  330.         configure_stream("SYS_STATUS", 5.0f);
  331.         configure_stream("EXTENDED_SYS_STATE", 1.0f);
  332.         configure_stream("ATTITUDE", 25.0f);
  333.         configure_stream("RC_CHANNELS", 5.0f);
  334.         configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
  335.         configure_stream("ALTITUDE", 1.0f);
  336.         configure_stream("GPS_RAW_INT", 1.0f);
  337.         configure_stream("ESTIMATOR_STATUS", 1.0f);
  338.         configure_stream("GLOBAL_POSITION_INT", 10.0f);
  339.         configure_stream("ATTITUDE_TARGET", 10.0f);
  340.         configure_stream("HOME_POSITION", 0.5f);
  341.         configure_stream("VFR_HUD", 25.0f);
  342.         configure_stream("WIND_COV", 2.0f);
  343.         configure_stream("SYSTEM_TIME", 1.0f);
  344.         break;

  346.     case MAVLINK_MODE_MAGIC:
  347.         //stream nothing
  348.         break;

  350.     case MAVLINK_MODE_CONFIG:
  351.         // Enable a number of interesting streams we want via USB
  352.         configure_stream("SYS_STATUS", 1.0f);
  353.         configure_stream("EXTENDED_SYS_STATE", 2.0f);
  354.         configure_stream("HIGHRES_IMU", 50.0f);
  355.         configure_stream("ATTITUDE", 50.0f);
  356.         configure_stream("ATTITUDE_QUATERNION", 50.0f);
  357.         configure_stream("RC_CHANNELS", 10.0f);
  358.         configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
  359.         configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
  360.         configure_stream("ALTITUDE", 10.0f);
  361.         configure_stream("GPS_RAW_INT", 10.0f);
  362.         configure_stream("ADSB_VEHICLE", 20.0f);
  363.         configure_stream("COLLISION", 20.0f);
  364.         configure_stream("DISTANCE_SENSOR", 10.0f);
  365.         configure_stream("OPTICAL_FLOW_RAD", 10.0f);
  366.         configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
  367.         configure_stream("ESTIMATOR_STATUS", 5.0f);
  368.         configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
  369.         configure_stream("GLOBAL_POSITION_INT", 10.0f);
  370.         configure_stream("LOCAL_POSITION_NED", 30.0f);
  371.         configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
  372.         configure_stream("ATTITUDE_TARGET", 8.0f);
  373.         configure_stream("HOME_POSITION", 0.5f);
  374.         configure_stream("NAMED_VALUE_FLOAT", 50.0f);
  375.         configure_stream("VFR_HUD", 20.0f);
  376.         configure_stream("WIND_COV", 10.0f);
  377.         configure_stream("CAMERA_TRIGGER", 500.0f);
  378.         configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
  379.         configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
  380.         configure_stream("MANUAL_CONTROL", 5.0f);
  381.         break;

  383.     case MAVLINK_MODE_IRIDIUM:
  384.         configure_stream("HIGH_LATENCY", 0.1f);
  385.         break;

  387.     default:
  388.         break;
  389.     }

  391.     /* set main loop delay depending on data rate to minimize CPU overhead */                                            //根据data reate减少CPU开销,设置主要loop延迟
  392.     _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;

  394.     /* hard limit to 500 Hz at max */                                                                                    //硬界限到500Hz最大
  395.     if (_main_loop_delay < 2000) {
  396.         _main_loop_delay = 2000;
  397.     }

  399.     /* hard limit to 100 Hz at least */                                                                                  //最小100Hz
  400.     if (_main_loop_delay > 10000) {
  401.         _main_loop_delay = 10000;
  402.     }

  404.     /* now the instance is fully initialized and we can bump the instance count */                                       //现在实例完全初始化，并且我们能bump实例计数
  405.     LL_APPEND(_mavlink_instances, this);

  407.     /* init socket if necessary */                                                                                        //如果需要初始化socket
  408.     if (get_protocol() == UDP) {
  409.         init_udp();
  410.     }

  412.     /* if the protocol is serial, we send the system version blindly */                                                    //如果协议是攒口，发送系统版本blindy(盲目的）
  413.     if (get_protocol() == SERIAL) {
  414.         send_autopilot_capabilites();
  415.     }

  417.     while (!_task_should_exit) {    
  418.         /* main loop */                                                                                                //主循环
  419.         usleep(_main_loop_delay);

  421.         perf_begin(_loop_perf);

  423.         hrt_abstime t = hrt_absolute_time();

  425.         update_rate_mult();

  427.         if (param_sub->update(&param_time, nullptr)) {
  428.             /* parameters updated */                                                                                //参数更新
  429.             mavlink_update_system();
  430.         }

  432.         /* radio config check */                                                                                    //比例设置确认
  433.         if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
  434.             /* request to configure radio and radio is present */
  435.             FILE *fs = fdopen(_uart_fd, "w");

  437.             if (fs) {
  438.                 /* switch to AT command mode */                                                                    //转换到AT命令模式
  439.                 usleep(1200000);
  440.                 fprintf(fs, "+++ ");
  441.                 usleep(1200000);

  443.                 if (_radio_id > 0) {
  444.                     /* set channel */
  445.                     fprintf(fs, "ATS3=%u ", _radio_id);
  446.                     usleep(200000);

  448.                 } else {
  449.                     /* reset to factory defaults */                                                //重置factory defaults
  450.                     fprintf(fs, "AT&F ");
  451.                     usleep(200000);
  452.                 }

  454.                 /* write config */                                                                //写配置
  455.                 fprintf(fs, "AT&W");
  456.                 usleep(200000);

  458.                 /* reboot */                                                                        //重启
  459.                 fprintf(fs, "ATZ");
  460.                 usleep(200000);

  462.                 // XXX NuttX suffers from a bug where
  463.                 // fclose() also closes the fd, not just
  464.                 // the file stream. Since this is a one-time
  465.                 // config thing, we leave the file struct
  466.                 // allocated.
  467. #ifndef __PX4_NUTTX
  468.                 fclose(fs);
  469. #endif

  471.             } else {
  472.                 PX4_WARN("open fd %d failed", _uart_fd);
  473.             }

  475.             /* reset param and save */                                                                    //重设param并且保存
  476.             _radio_id = 0;
  477.             param_set(_param_radio_id, &_radio_id);
  478.         }

  480.         if (status_sub->update(&status_time, &status)) {
  481.             /* switch HIL mode if required */                                                             //转换HIL模式如果需要
  482.             set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);

  484.             set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
  485.         }

  487.         /* send command ACK */                                                                            //发送ACK命令
  488.         uint16_t current_command_ack = 0;

  490.         if (ack_sub->update(&ack_time, &command_ack)) {
  491.             mavlink_command_ack_t msg;
  492.             msg.result = command_ack.result;
  493.             msg.command = command_ack.command;
  494.             current_command_ack = command_ack.command;

  496.             mavlink_msg_command_ack_send_struct(get_channel(), &msg);
  497.         }

  499.         struct mavlink_log_s mavlink_log;

  501.         if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
  502.             _logbuffer.put(&mavlink_log);
  503.         }

  505.         /* check for shell output */                                                                                    //确认作为shell输出
  506.         if (_mavlink_shell && _mavlink_shell->available() > 0) {
  507.             if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
  508.                 mavlink_serial_control_t msg;
  509.                 msg.baudrate = 0;
  510.                 msg.flags = SERIAL_CONTROL_FLAG_REPLY;
  511.                 msg.timeout = 0;
  512.                 msg.device = SERIAL_CONTROL_DEV_SHELL;
  513.                 msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
  514.                 mavlink_msg_serial_control_send_struct(get_channel(), &msg);
  515.             }
  516.         }

  518.         /* check for ulog streaming messages */                                                                        //确认作为ulog streaming消息
  519.         if (_mavlink_ulog) {
  520.             if (_mavlink_ulog_stop_requested) {
  521.                 _mavlink_ulog->stop();
  522.                 _mavlink_ulog = nullptr;
  523.                 _mavlink_ulog_stop_requested = false;

  525.             } else {
  526.                 if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
  527.                     _mavlink_ulog->start_ack_received();
  528.                 }

  530.                 int ret = _mavlink_ulog->handle_update(get_channel());

  532.                 if (ret < 0) { //abort the streaming on error
  533.                     if (ret != -1) {
  534.                         PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
  535.                     }

  537.                     _mavlink_ulog->stop();
  538.                     _mavlink_ulog = nullptr;
  539.                 }
  540.             }
  541.         }

  543.         /* check for requested subscriptions */                                                                    //确认requested subscriptions
  544.         if (_subscribe_to_stream != nullptr) {
  545.             if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
  546.                 if (_subscribe_to_stream_rate > 0.0f) {
  547.                     if (get_protocol() == SERIAL) {
  548.                         PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
  549.                              (double)_subscribe_to_stream_rate);

  551.                     } else if (get_protocol() == UDP) {
  552.                         PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
  553.                              (double)_subscribe_to_stream_rate);
  554.                     }

  556.                 } else {
  557.                     if (get_protocol() == SERIAL) {
  558.                         PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);

  560.                     } else if (get_protocol() == UDP) {
  561.                         PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
  562.                     }
  563.                 }

  565.             } else {
  566.                 if (get_protocol() == SERIAL) {
  567.                     PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);

  569.                 } else if (get_protocol() == UDP) {
  570.                     PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
  571.                 }
  572.             }

  574.             _subscribe_to_stream = nullptr;
  575.         }

  577.         /* update streams */                                                                                                     //更新流
  578.         MavlinkStream *stream;
  579.         LL_FOREACH(_streams, stream) {
  580.             stream->update(t);
  581.         }

  583.         /* pass messages from other UARTs or FTP worker */                                                                    //传递消息从其他UARTs或FTP 队列
  584.         if (_forwarding_on || _ftp_on) {

  586.             bool is_part;
  587.             uint8_t *read_ptr;
  588.             uint8_t *write_ptr;

  590.             pthread_mutex_lock(&_message_buffer_mutex);
  591.             int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
  592.             pthread_mutex_unlock(&_message_buffer_mutex);

  594.             if (available > 0) {
  595.                 // Reconstruct message from buffer

  597.                 mavlink_message_t msg;
  598.                 write_ptr = (uint8_t *)&msg;

  600.                 // Pull a single message from the buffer
  601.                 size_t read_count = available;

  603.                 if (read_count > sizeof(mavlink_message_t)) {
  604.                     read_count = sizeof(mavlink_message_t);
  605.                 }

  607.                 memcpy(write_ptr, read_ptr, read_count);

  609.                 // We hold the mutex until after we complete the second part of the buffer. If we don

无欲速，无见小利。欲速，则不达；见小利，则大事不成。
【推广】 免费学中医，健康全家人
原文地址：https://www.cnblogs.com/ch122633/p/7363239.html