fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 03:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor replay: move topic publication into separate method

Browse Source
This commit is contained in:
Beat Küng 2017-03-01 13:20:03 +01:00 committed by Lorenz Meier
parent 45ffb190e3
commit 4eea89bb42
2 changed files with 56 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/replay/replay.hpp
View File

@ -61,11 +61,13 @@ public:
/// Destructor, also waits for task exit
~Replay();
/// Start task.
/// @param quiet silently fail if no log file found
/// @param apply_params_only if true, only apply parameters from definitions section of the file
/// and user-overridden parameters, then exit w/o replaying.
/// @return OK on success.
/**
* Start task.
* @param quiet silently fail if no log file found
* @param apply_params_only if true, only apply parameters from definitions section of the file
* and user-overridden parameters, then exit w/o replaying.
* @return OK on success.
*/
static int start(bool quiet, bool apply_params_only);
static void task_main_trampoline(int argc, char *argv[]);
@ -155,6 +157,14 @@ private:
void setUserParams(const char *filename);
/**
* publish an orb topic
* @param sub
* @param data
* @return true if published, false otherwise
*/
bool publishTopic(Subscription &sub, void *data);
static char *_replay_file;
};

69
src/modules/replay/replay_main.cpp
View File

@ -702,35 +702,8 @@ void Replay::task_main()
replay_file.read((char *)_read_buffer.data(), msg_read_size);
*(uint64_t *)(_read_buffer.data() + sub.timestamp_offset) = publish_timestamp;
if (sub.orb_advert) {
orb_publish(sub.orb_meta, sub.orb_advert, _read_buffer.data());
if (publishTopic(sub, _read_buffer.data())) {
++nr_published_messages;
} else {
if (sub.multi_id == 0) {
sub.orb_advert = orb_advertise(sub.orb_meta, _read_buffer.data());
++nr_published_messages;
} else {
// make sure the other instances are advertised already so that we get the correct instance
bool advertised = false;
for (const auto &subscription : _subscriptions) {
if (subscription.orb_meta) {
if (strcmp(sub.orb_meta->o_name, subscription.orb_meta->o_name) == 0 &&
subscription.orb_advert && subscription.multi_id == sub.multi_id - 1) {
advertised = true;
}
}
}
if (advertised) {
int instance;
sub.orb_advert = orb_advertise_multi(sub.orb_meta, _read_buffer.data(),
&instance, ORB_PRIO_DEFAULT);
++nr_published_messages;
}
}
}
@ -754,6 +727,46 @@ void Replay::task_main()
}
}
bool Replay::publishTopic(Subscription &sub, void *data)
{
bool published = false;
if (sub.orb_advert) {
orb_publish(sub.orb_meta, sub.orb_advert, _read_buffer.data());
published = true;
} else {
if (sub.multi_id == 0) {
sub.orb_advert = orb_advertise(sub.orb_meta, _read_buffer.data());
published = true;
} else {
// make sure the other instances are advertised already so that we get the correct instance
bool advertised = false;
for (const auto &subscription : _subscriptions) {
if (subscription.orb_meta) {
if (strcmp(sub.orb_meta->o_name, subscription.orb_meta->o_name) == 0 &&
subscription.orb_advert && subscription.multi_id == sub.multi_id - 1) {
advertised = true;
}
}
}
if (advertised) {
int instance;
sub.orb_advert = orb_advertise_multi(sub.orb_meta, _read_buffer.data(),
&instance, ORB_PRIO_DEFAULT);
published = true;
}
}
}
return published;
}
}
void Replay::task_main_trampoline(int argc, char *argv[])
{
replay::instance = new Replay();