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

Posted 2022-08-11 lida2003

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

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

1. Replay模块简介

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

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

1. trystart
2. 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();

4. 总结

整体看下来，也并不复杂，仅仅只是飞控姿态的回放。没有太多其他资料信息，比如：GPS，指令（GCS，RC），空气动力学模拟信息等。

如果能将这个黑匣子数据结合到模拟器上，进行重飞，那就更有意义了。

5. 参考资料

【1】PX4开源软件框架简明简介
【2】PX4 Replay模块
【3】PX4模块设计之十五：PX4 Log设计