Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

replay: some API refactoring & extensions

Browse Source 
in preparation to the updated ekf2 replay
This commit is contained in:
Beat Küng
2017-03-03 08:21:41 +01:00
committed by Lorenz Meier
parent ba89839f65
commit 473192aa81
3 changed files with 50 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
posix-configs/SITL/init/ekf2/iris_replay
+1 -1
View File
@@ -3,5 +3,5 @@ uorb start
ekf2 start --replay
logger start -e -t -b 1000 -p vehicle_attitude
sleep 0.2
replay start -m ekf2
replay start
src/modules/replay/replay.hpp
+28 -15
View File
@@ -93,6 +93,8 @@ protected:
uint8_t multi_id;
int timestamp_offset; ///< marks the field of the timestamp
bool ignored = false; ///< if true, it will not be considered for publication in the main loop
std::streampos next_read_pos;
uint64_t next_timestamp; ///< timestamp of the file
};
@@ -114,6 +116,11 @@ protected:
*/
virtual void onExitMainLoop() {};
/**
* called when a new subscription is added
*/
virtual void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) {};
/**
* handle delay until topic can be published.
* @param next_file_timestamp timestamp of next message to publish
@@ -126,7 +133,25 @@ protected:
* handle the publication of a topic update
* @return true if published, false otherwise
*/
virtual bool handleTopicUpdate(Subscription &sub, void *data);
virtual bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file);
/**
* read a topic from the file (offset given by the subscription) into _read_buffer
*/
void readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_file);
/**
* Find next data message for this subscription, starting with the stored file offset.
* Skip the first message, and if found, read the timestamp and store the new file offset.
* This also takes care of new subscriptions and parameter updates. When reaching EOF,
* the subscription is set to invalid.
* File seek position is arbitrary after this call.
* @return false on file error
*/
bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
std::vector<Subscription> _subscriptions;
std::vector<uint8_t> _read_buffer;
private:
bool _task_should_exit = false;
@@ -136,9 +161,6 @@ private:
uint64_t _file_start_time;
uint64_t _replay_start_time;
std::streampos _data_section_start; ///< first ADD_LOGGED_MSG message
std::vector<uint8_t> _read_buffer;
std::vector<Subscription> _subscriptions;
/** keep track of file position to avoid adding a subscription multiple times. */
std::streampos _subscription_file_pos = 0;
@@ -173,16 +195,6 @@ private:
bool readDropout(std::ifstream &file, uint16_t msg_size);
bool readAndApplyParameter(std::ifstream &file, uint16_t msg_size);
/**
* Find next data message for this subscription, starting with the stored file offset.
* Skip the first message, and if found, read the timestamp and store the new file offset.
* This also takes care of new subscriptions and parameter updates. When reaching EOF,
* the subscription is set to invalid.
* File seek position is arbitrary after this call.
* @return false on file error
*/
bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
static const orb_metadata *findTopic(const std::string &name);
/** get the array size from a type. eg. float[3] -> return float */
static std::string extractArraySize(const std::string &type_name_full, int &array_size);
@@ -217,7 +229,8 @@ protected:
* @param data
* @return true if published, false otherwise
*/
bool handleTopicUpdate(Subscription &sub, void *data) override;
bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override;
private:
src/modules/replay/replay_main.cpp
+21 -15
View File
@@ -371,6 +371,8 @@ bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
_subscriptions[msg_id] = subscription;
onSubscriptionAdded(_subscriptions[msg_id], msg_id);
return true;
}
@@ -672,7 +674,7 @@ void Replay::task_main()
for (size_t i = 0; i < _subscriptions.size(); ++i) {
const Subscription &subscription = _subscriptions[i];
if (subscription.orb_meta) {
if (subscription.orb_meta && !subscription.ignored) {
if (next_file_time == 0 || subscription.next_timestamp < next_file_time) {
next_msg_id = (int)i;
next_file_time = subscription.next_timestamp;
@@ -704,18 +706,14 @@ void Replay::task_main()
//It's time to publish
const size_t msg_read_size = sub.orb_meta->o_size_no_padding;
const size_t msg_write_size = sub.orb_meta->o_size;
_read_buffer.reserve(msg_write_size);
replay_file.seekg(sub.next_read_pos + (streamoff)(ULOG_MSG_HEADER_LEN + 2)); //skip header & msg id
replay_file.read((char *)_read_buffer.data(), msg_read_size);
*(uint64_t *)(_read_buffer.data() + sub.timestamp_offset) = publish_timestamp;
readTopicDataToBuffer(sub, replay_file);
memcpy(_read_buffer.data() + sub.timestamp_offset, &publish_timestamp, sizeof(uint64_t)); //adjust the timestamp
if (handleTopicUpdate(sub, _read_buffer.data())) {
if (handleTopicUpdate(sub, _read_buffer.data(), replay_file)) {
++nr_published_messages;
}
nextDataMessage(replay_file, _subscriptions[next_msg_id], next_msg_id);
nextDataMessage(replay_file, sub, next_msg_id);
//TODO: output status (eg. every sec), including total duration...
}
@@ -737,7 +735,16 @@ void Replay::task_main()
onExitMainLoop();
}
bool Replay::handleTopicUpdate(Subscription &sub, void *data)
void Replay::readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_file)
{
const size_t msg_read_size = sub.orb_meta->o_size_no_padding;
const size_t msg_write_size = sub.orb_meta->o_size;
_read_buffer.reserve(msg_write_size);
replay_file.seekg(sub.next_read_pos + (streamoff)(ULOG_MSG_HEADER_LEN + 2)); //skip header & msg id
replay_file.read((char *)_read_buffer.data(), msg_read_size);
}
bool Replay::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
{
return publishTopic(sub, data);
}
@@ -763,12 +770,12 @@ 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());
orb_publish(sub.orb_meta, sub.orb_advert, data);
published = true;
} else {
if (sub.multi_id == 0) {
sub.orb_advert = orb_advertise(sub.orb_meta, _read_buffer.data());
sub.orb_advert = orb_advertise(sub.orb_meta, data);
published = true;
} else {
@@ -786,8 +793,7 @@ bool Replay::publishTopic(Subscription &sub, void *data)
if (advertised) {
int instance;
sub.orb_advert = orb_advertise_multi(sub.orb_meta, _read_buffer.data(),
&instance, ORB_PRIO_DEFAULT);
sub.orb_advert = orb_advertise_multi(sub.orb_meta, data, &instance, ORB_PRIO_DEFAULT);
published = true;
}
}
@@ -796,7 +802,7 @@ bool Replay::publishTopic(Subscription &sub, void *data)
return published;
}
bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data)
bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
{
if (sub.orb_meta == ORB_ID(ekf2_replay)) {
struct ekf2_replay_s ekf2_replay;