PX4模块设计之十九：Replay模块

最新推荐文章于 2024-09-07 08:42:15 发布

lida2003

最新推荐文章于 2024-09-07 08:42:15 发布

阅读量758

点赞数

分类专栏： PX4 文章标签： px4 单片机嵌入式硬件

本文链接：https://blog.csdn.net/lida2003/article/details/126225213

版权

PX4 专栏收录该内容

53 篇文章 219 订阅

订阅专栏

PX4模块设计之十九：Replay模块

1. Replay模块简介
2. 模块入口函数
- 2.1 主入口replay_main
- 2.2 自定义子命令Replay::custom_command
3. 重要实现函数
4. 总结
5. 参考资料

1. Replay模块简介

Replay模块同样继承ModuleBase模块的基本命令start/stop/status命令。

此外增加了两个自定义子命令：

trystart
tryapplyparams

Replay模块主要就是回放的功能，将飞控的飞行姿态根据播放速率重新回放一遍。

pxh> replay

### Description
This module is used to replay ULog files.

There are 2 environment variables used for configuration: `
Usage: replay <command> [arguments...]
 Commands:

   start         Start replay, using log file from ENV variable 'replay'

   trystart      Same as 'start', but silently exit if no log file given

   tryapplyparams Try to apply the parameters from the log file

   stop

   status        print status info

注：上述打印帮助来自函数Replay::print_usage，具体Coding这里不再赘述，有兴趣的同学可以独立深入。

2. 模块入口函数

2.1 主入口replay_main

replay_main
 ├──> const char *logfile = getenv(replay::ENV_FILENAME);
 ├──> <logfile && !Replay::isSetup()>
 │   ├──> PX4_INFO("using replay log file: %s", logfile);
 │   └──> Replay::setupReplayFile(logfile);
 └──> return Replay::main(argc, argv);  // 调用ModuleBase的main方法

注：具体入口后实现详见【PX4模块设计之十七：ModuleBase模块】

2.2 自定义子命令Replay::custom_command

Replay::custom_command
 ├──> <!strcmp(argv[0], "tryapplyparams")>
 │   └──> return Replay::applyParams(true);
 ├──> <!strcmp(argv[0], "trystart")>
 │   └──> return Replay::task_spawn(argc, argv);
 └──> return print_usage("unknown command");

3. 重要实现函数

3.1 Replay::task_spawn

Replay::task_spawn
 ├──> <!isSetup()> // check if a log file was found
 │   ├──> <argc > 0 && strncmp(argv[0], "try", 3) == 0>
 │   │   └──> return 0;
 │   ├──> PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME);
 │   └──> return -1;
 ├──> _task_id = px4_task_spawn_cmd("replay", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, (px4_main_t)&run_trampoline, (char *const *)argv);
 ├──> <_task_id < 0>
 │   ├──> _task_id = -1;
 │   └──> return -errno;
 └──> return 0;

3.2 Replay::instantiate

Replay::instantiate
 ├──> const char *replay_mode = getenv(replay::ENV_MODE);
 ├──> Replay *instance = nullptr;
 ├──> <replay_mode && strcmp(replay_mode, "ekf2") == 0>
 │   ├──> PX4_INFO("Ekf2 replay mode");
 │   └──> instance = new ReplayEkf2();
 ├──> <else>
 │   └──> instance = new Replay();
 └──> return instance;

3.3 Replay::run

主要订阅回放的是"sensor_combined"uORB消息，通过传感器反馈数据对飞行姿态进行回放。

Replay::run
 ├──> ifstream replay_file(_replay_file, ios::in | ios::binary);
 ├──> <!readDefinitionsAndApplyParams(replay_file)>
 │   └──> return
 ├──> _speed_factor = 1.f;
 ├──> const char *speedup = getenv("PX4_SIM_SPEED_FACTOR");
 ├──> <speedup>
 │   └──> _speed_factor = atof(speedup);
 ├──> onEnterMainLoop();
 ├──> _replay_start_time = hrt_absolute_time();
 ├──> PX4_INFO("Replay in progress...");
 ├──> replay_file.seekg(_data_section_start);
 ├──> replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); //we know the next message must be an ADD_LOGGED_MSG
 ├──> <!readAndAddSubscription(replay_file, message_header.msg_size)>
 │   ├──> <PX4_ERR("Failed to read subscription");>
 │   └──> return
 ├──> const uint64_t timestamp_offset = getTimestampOffset();
 ├──> uint32_t nr_published_messages = 0;
 ├──> streampos last_additional_message_pos = _data_section_start;  //给出了数据日志记录的起始位置
 ├──> <while (!should_exit() && replay_file) >
 │   ├──> [readAndAddSubscription:检查"sensor_combined"主题消息]
 │   ├──> [nextDataMessage：读取内容"sensor_combined"主题消息内容]
 │   ├──> [handleTopicDelay：根据发布时间，replay便宜时间，以及replay speedup等因素，调整uORB消息发布时间]
 │   └──> [handleTopicUpdate：发布uORB消息]
 ├──> <for (auto &subscription : _subscriptions)>
 │   ├──> <!subscription>
 │   │   └──> continue
 │   ├──> <subscription->compat>
 │   │   ├──> delete subscription->compat;
 │   │   └──> subscription->compat = nullptr;
 │   └──> <subscription->orb_advert>
 │       ├──> orb_unadvertise(subscription->orb_advert);
 │       └──> subscription->orb_advert = nullptr;
 ├──> <!should_exit()>
 │   └──> PX4_INFO("Replay done (published %u msgs, %.3lf s)", nr_published_messages, (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
 ├──> onExitMainLoop();
 └──> <!should_exit()>
     ├──> replay_file.close();
     └──> px4_shutdown_request();