zoukankan      html  css  js  c++  java

  • 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

    无欲速，无见小利。欲速，则不达；见小利，则大事不成。
  • 相关阅读:
    CodeBlocks下载与安装教程
    Delphi 资源管理器套件
    做了一个 62 进制的简单实现
    关于 TRegEx.Split()
    Delphi 的链式代码
    在Linux上编译dotnet cli的源代码生成.NET Core SDK的安装包
    尝试解决在构造函数中同步调用Dns.GetHostAddressesAsync()引起的线程死锁
    .NET Core中遇到奇怪的线程死锁问题：内存与线程数不停地增长
    将asp.net core站点发布到IIS上遇到的问题
    .NET Core 构建配置文件从 project.json 到 .csproj
  • 原文地址：https://www.cnblogs.com/ch122633/p/7363239.html
Copyright © 2011-2022 走看看