Mavlink_main.cpp源码学习

最新推荐文章于 2024-09-11 09:41:37 发布

习惯就好zz

最新推荐文章于 2024-09-11 09:41:37 发布

阅读量1.4k

点赞数

分类专栏： # C/C++ 文章标签：自动驾驶 qt c++

本文为博主原创文章，未经博主允许不得转载。

本文链接：https://blog.csdn.net/ch122633/article/details/77176529

版权

C/C++ 专栏收录该内容

22 篇文章 0 订阅

订阅专栏

int mavlink_main(int argc, char *argv[])
{
if (argc < 2) {
usage(); //使用说明
return 1;
}
if (!strcmp(argv[1], "start")) { //在这里启动了mavlink
return Mavlink::start(argc, argv);
} else if (!strcmp(argv[1], "stop")) { //命令是stop-all
PX4_WARN("mavlink stop is deprecated, use stop-all instead");
usage();
return 1;
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances(); //关闭start里创建的任务
} else if (!strcmp(argv[1], "status")) {
return Mavlink::get_status_all_instances(); //打印start中创建的任务信息
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream_command(argc, argv);
} else if (!strcmp(argv[1], "boot_complete")) {
Mavlink::set_boot_complete();
return 0;
} else {
usage();
return 1;
}
return 0;
}

int
Mavlink::start(int argc, char *argv[])
{
MavlinkULog::initialize();
MavlinkCommandSender::initialize();
// Wait for the instance count to go up one
// before returning to the shell
int ic = Mavlink::instance_count(); //确认不超过最大个数
if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
warnx("Maximum MAVLink instance count of %d reached.",
(int)Mavlink::MAVLINK_MAX_INSTANCES);
return 1;
}
// Instantiate thread
char buf[24];
sprintf(buf, "mavlink_if%d", ic);
// This is where the control flow splits
// between the starting task and the spawned
// task - start_helper() only returns
// when the started task exits. //这里创建了start_helper的守护进程
px4_task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2500,
(px4_main_t)&Mavlink::start_helper,
(char *const *)argv);
// Ensure that this shell command
// does not return before the instance
// is fully initialized. As this is also
// the only path to create a new instance,
// this is effectively a lock on concurrent
// instance starting. XXX do a real lock.
// Sleep 500 us between each attempt
const unsigned sleeptime = 500;
// Wait 100 ms max for the startup.
const unsigned limit = 100 * 1000 / sleeptime;
unsigned count = 0;
while (ic == Mavlink::instance_count() && count < limit) { //轮询等待
::usleep(sleeptime);
count++;
}
return OK;
}

int
Mavlink::task_main(int argc, char *argv[])
{
int ch;
_baudrate = 57600;
_datarate = 0;
_mode = MAVLINK_MODE_NORMAL;
#ifdef __PX4_NUTTX
/* the NuttX optarg handler does not
* ignore argv[0] like the POSIX handler
* does, nor does it deal with non-flag
* verbs well. So we remove the application
* name and the verb.
*/
argc -= 2;
argv += 2;
#endif
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
int myoptind = 1;
const char *myoptarg = nullptr;
#ifdef __PX4_POSIX
char *eptr;
int temp_int_arg;
#endif
while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fvwx", &myoptind, &myoptarg)) != EOF) { //这里把argv[0]后面的都反了一下
switch (ch) {
case 'b':
_baudrate = strtoul(myoptarg, nullptr, 10); //设置波特率
if (_baudrate < 9600 || _baudrate > 3000000) {
warnx("invalid baud rate '%s'", myoptarg);
err_flag = true;
}
break;
case 'r':
_datarate = strtoul(myoptarg, nullptr, 10); //设置传输速率
if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
warnx("invalid data rate '%s'", myoptarg);
err_flag = true;
}
break;
case 'd':
_device_name = myoptarg; //选择串口设备
set_protocol(SERIAL);
break;
#ifdef __PX4_POSIX
case 'u': //选择UDP网络本地
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if (*eptr == '\0') {
_network_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid data udp_port '%s'", myoptarg);
err_flag = true;
}
break;
case 'o': //选择UDP网络远程
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if (*eptr == '\0') {
_remote_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid remote udp_port '%s'", myoptarg);
err_flag = true;
}
break;
case 't': //伙伴IP，通过MAV_BROADCAST参数广播
_src_addr.sin_family = AF_INET;
if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
_src_addr_initialized = true;
} else {
warnx("invalid partner ip '%s'", myoptarg);
err_flag = true;
}
break;
#else
case 'u':
case 'o':
case 't':
warnx("UDP options not supported on this platform");
err_flag = true;
break;
#endif
// case 'e':
// mavlink_link_termination_allowed = true;
// break;
case 'm': //设置默认的stream和rates
if (strcmp(myoptarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(myoptarg, "camera") == 0) {
// left in here for compatibility
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(myoptarg, "onboard") == 0) {
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(myoptarg, "osd") == 0) {
_mode = MAVLINK_MODE_OSD;
} else if (strcmp(myoptarg, "magic") == 0) {
_mode = MAVLINK_MODE_MAGIC;
} else if (strcmp(myoptarg, "config") == 0) {
_mode = MAVLINK_MODE_CONFIG;
} else if (strcmp(myoptarg, "iridium") == 0) {
_mode = MAVLINK_MODE_IRIDIUM;
_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
}
break;
case 'f': //消息可发给其他mavlink接口
_forwarding_on = true;
break;
case 'v': //verbos output
_verbose = true;
break;
case 'w': //等待发送直到接收到第一个消息
_wait_to_transmit = true;
break;
case 'x': //开启ftp
_ftp_on = true;
break;
default: //其他应该是错误的
err_flag = true;
break;
}
}
if (err_flag) {
usage();
return PX4_ERROR;
}
if (_datarate == 0) {
/* convert bits to bytes and use 1/2 of bandwidth by default */
_datarate = _baudrate / 20;
}
if (_datarate > MAX_DATA_RATE) {
_datarate = MAX_DATA_RATE;
}
if (get_protocol() == SERIAL) {
if (Mavlink::instance_exists(_device_name, this)) {
warnx("%s already running", _device_name);
return PX4_ERROR;
}
PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
mavlink_mode_str(_mode), _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */ //清空输出缓冲区，并把缓冲区内容输出
fflush(stdout);
/* default values for arguments */ //默认的串口值
_uart_fd = mavlink_open_uart(_baudrate, _device_name);
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name);
return PX4_ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */
return OK;
}
} else if (get_protocol() == UDP) {
if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
warnx("port %d already occupied", _network_port);
return PX4_ERROR;
}
PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
mavlink_mode_str(_mode), _datarate, _network_port, _remote_port);
}
/* initialize send mutex */ //初始化发送mutex
pthread_mutex_init(&_send_mutex, nullptr);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ //如果我们在传送mavlink消息，我们需要准备一个buffer作为接口
if (_forwarding_on || _ftp_on) {
/* initialize message buffer if multiplexing is on or its needed for FTP. //如果multiplexing打开了或者需要FTP，初始化消息buffer。
* make space for two messages plus off-by-one space as we use the empty element //为两个消息提供足够的空间，并加上off-by-one空间，作为我们使用的空元素
* marker ring buffer approach. //标记ring buffer方法
*/
if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { //起始申请了3个message空间，第三个应该是空的。防止off-by-one
warnx("msg buf:");
return 1;
}
/* initialize message buffer mutex */ //初始化message buffer mutex
pthread_mutex_init(&_message_buffer_mutex, nullptr);
}
/* Initialize system properties */ //初始化系统内容
mavlink_update_system();
/* start the MAVLink receiver */ //开始接收mavlink
MavlinkReceiver::receive_start(&_receive_thread, this);
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0;
MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
/* We don't want to miss the first advertise of an ACK, so we subscribe from the //我们不想要丢失最开始ACK的advertise，所以我们topic退出后，还会发送一段时间
* beginning and not just when the topic exists. */
ack_sub->subscribe_from_beginning(true);
uint64_t ack_time = 0;
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
struct vehicle_command_ack_s command_ack;
ack_sub->update(&ack_time, &command_ack);
/* add default streams depending on mode */ //根据mode,增加默认的streams
if (_mode != MAVLINK_MODE_IRIDIUM) {
/* HEARTBEAT is constant rate stream, rate never adjusted */
configure_stream("HEARTBEAT", 1.0f);
/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
configure_stream("STATUSTEXT", 20.0f);
/* COMMAND_LONG stream: use high rate to avoid commands skipping */
configure_stream("COMMAND_LONG", 100.0f);
}
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("EXTENDED_SYS_STATE", 1.0f);
configure_stream("HIGHRES_IMU", 1.5f);
configure_stream("ATTITUDE", 20.0f);
configure_stream("RC_CHANNELS", 5.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream("ALTITUDE", 1.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("ADSB_VEHICLE", 2.0f);
configure_stream("COLLISION", 2.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW_RAD", 1.0f);
configure_stream("VISION_POSITION_ESTIMATE", 1.0f);
configure_stream("ESTIMATOR_STATUS", 0.5f);
configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
configure_stream("GLOBAL_POSITION_INT", 5.0f);
configure_stream("LOCAL_POSITION_NED", 1.0f);
configure_stream("POSITION_TARGET_LOCAL_NED", 1.5f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
configure_stream("ATTITUDE_TARGET", 2.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
configure_stream("VFR_HUD", 4.0f);
configure_stream("WIND_COV", 1.0f);
break;
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 5.0f);
configure_stream("EXTENDED_SYS_STATE", 5.0f);
configure_stream("HIGHRES_IMU", 50.0f);
configure_stream("ATTITUDE", 250.0f);
configure_stream("ATTITUDE_QUATERNION", 50.0f);
configure_stream("RC_CHANNELS", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
configure_stream("ALTITUDE", 10.0f);
configure_stream("GPS_RAW_INT", 5.0f);
configure_stream("ADSB_VEHICLE", 10.0f);
configure_stream("COLLISION", 10.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
configure_stream("ESTIMATOR_STATUS", 1.0f);
configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream("ATTITUDE_TARGET", 10.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("VFR_HUD", 10.0f);
configure_stream("WIND_COV", 10.0f);
configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
configure_stream("SYSTEM_TIME", 1.0f);
configure_stream("TIMESYNC", 10.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
//camera trigger is rate limited at the source, do not limit here
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
break;
case MAVLINK_MODE_OSD:
configure_stream("SYS_STATUS", 5.0f);
configure_stream("EXTENDED_SYS_STATE", 1.0f);
configure_stream("ATTITUDE", 25.0f);
configure_stream("RC_CHANNELS", 5.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream("ALTITUDE", 1.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("ESTIMATOR_STATUS", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 10.0f);
configure_stream("ATTITUDE_TARGET", 10.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("VFR_HUD", 25.0f);
configure_stream("WIND_COV", 2.0f);
configure_stream("SYSTEM_TIME", 1.0f);
break;
case MAVLINK_MODE_MAGIC:
//stream nothing
break;
case MAVLINK_MODE_CONFIG:
// Enable a number of interesting streams we want via USB
configure_stream("SYS_STATUS", 1.0f);
configure_stream("EXTENDED_SYS_STATE", 2.0f);
configure_stream("HIGHRES_IMU", 50.0f);
configure_stream("ATTITUDE", 50.0f);
configure_stream("ATTITUDE_QUATERNION", 50.0f);
configure_stream("RC_CHANNELS", 10.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream("ALTITUDE", 10.0f);
configure_stream("GPS_RAW_INT", 10.0f);
configure_stream("ADSB_VEHICLE", 20.0f);
configure_stream("COLLISION", 20.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
configure_stream("ESTIMATOR_STATUS", 5.0f);
configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
configure_stream("GLOBAL_POSITION_INT", 10.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream("ATTITUDE_TARGET", 8.0f);
configure_stream("HOME_POSITION", 0.5f);
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
configure_stream("VFR_HUD", 20.0f);
configure_stream("WIND_COV", 10.0f);
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
break;
case MAVLINK_MODE_IRIDIUM:
configure_stream("HIGH_LATENCY", 0.1f);
break;
default:
break;
}
/* set main loop delay depending on data rate to minimize CPU overhead */ //根据data reate减少CPU开销,设置主要loop延迟
_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
/* hard limit to 500 Hz at max */ //硬界限到500Hz最大
if (_main_loop_delay < 2000) {
_main_loop_delay = 2000;
}
/* hard limit to 100 Hz at least */ //最小100Hz
if (_main_loop_delay > 10000) {
_main_loop_delay = 10000;
}
/* now the instance is fully initialized and we can bump the instance count */ //现在实例完全初始化，并且我们能bump实例计数
LL_APPEND(_mavlink_instances, this);
/* init socket if necessary */ //如果需要初始化socket
if (get_protocol() == UDP) {
init_udp();
}
/* if the protocol is serial, we send the system version blindly */ //如果协议是攒口，发送系统版本blindy(盲目的）
if (get_protocol() == SERIAL) {
send_autopilot_capabilites();
}
while (!_task_should_exit) {
/* main loop */ //主循环
usleep(_main_loop_delay);
perf_begin(_loop_perf);
hrt_abstime t = hrt_absolute_time();
update_rate_mult();
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */ //参数更新
mavlink_update_system();
}
/* radio config check */ //比例设置确认
if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
if (fs) {
/* switch to AT command mode */ //转换到AT命令模式
usleep(1200000);
fprintf(fs, "+++\n");
usleep(1200000);
if (_radio_id > 0) {
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);
} else {
/* reset to factory defaults */ //重置factory defaults
fprintf(fs, "AT&F\n");
usleep(200000);
}
/* write config */ //写配置
fprintf(fs, "AT&W");
usleep(200000);
/* reboot */ //重启
fprintf(fs, "ATZ");
usleep(200000);
// XXX NuttX suffers from a bug where
// fclose() also closes the fd, not just
// the file stream. Since this is a one-time
// config thing, we leave the file struct
// allocated.
#ifndef __PX4_NUTTX
fclose(fs);
#endif
} else {
PX4_WARN("open fd %d failed", _uart_fd);
}
/* reset param and save */ //重设param并且保存
_radio_id = 0;
param_set(_param_radio_id, &_radio_id);
}
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */ //转换HIL模式如果需要
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
}
/* send command ACK */ //发送ACK命令
uint16_t current_command_ack = 0;
if (ack_sub->update(&ack_time, &command_ack)) {
mavlink_command_ack_t msg;
msg.result = command_ack.result;
msg.command = command_ack.command;
current_command_ack = command_ack.command;
mavlink_msg_command_ack_send_struct(get_channel(), &msg);
}
struct mavlink_log_s mavlink_log;
if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
_logbuffer.put(&mavlink_log);
}
/* check for shell output */ //确认作为shell输出
if (_mavlink_shell && _mavlink_shell->available() > 0) {
if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
mavlink_serial_control_t msg;
msg.baudrate = 0;
msg.flags = SERIAL_CONTROL_FLAG_REPLY;
msg.timeout = 0;
msg.device = SERIAL_CONTROL_DEV_SHELL;
msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
mavlink_msg_serial_control_send_struct(get_channel(), &msg);
}
}
/* check for ulog streaming messages */ //确认作为ulog streaming消息
if (_mavlink_ulog) {
if (_mavlink_ulog_stop_requested) {
_mavlink_ulog->stop();
_mavlink_ulog = nullptr;
_mavlink_ulog_stop_requested = false;
} else {
if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
_mavlink_ulog->start_ack_received();
}
int ret = _mavlink_ulog->handle_update(get_channel());
if (ret < 0) { //abort the streaming on error
if (ret != -1) {
PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
}
_mavlink_ulog->stop();
_mavlink_ulog = nullptr;
}
}
}
/* check for requested subscriptions */ //确认requested subscriptions
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
if (get_protocol() == SERIAL) {
PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate);
} else if (get_protocol() == UDP) {
PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
(double)_subscribe_to_stream_rate);
}
} else {
if (get_protocol() == SERIAL) {
PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
} else if (get_protocol() == UDP) {
PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
}
}
} else {
if (get_protocol() == SERIAL) {
PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);
} else if (get_protocol() == UDP) {
PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
}
}
_subscribe_to_stream = nullptr;
}
/* update streams */ //更新流
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
stream->update(t);
}
/* pass messages from other UARTs or FTP worker */ //传递消息从其他UARTs或FTP 队列
if (_forwarding_on || _ftp_on) {
bool is_part;
uint8_t *read_ptr;
uint8_t *write_ptr;
pthread_mutex_lock(&_message_buffer_mutex);
int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
pthread_mutex_unlock(&_message_buffer_mutex);
if (available > 0) {
// Reconstruct message from buffer
mavlink_message_t msg;
write_ptr = (uint8_t *)&msg;
// Pull a single message from the buffer
size_t read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don