diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 6218b09e00..3f05082f9a 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -65,6 +65,7 @@ #include "ReplayEkf2.hpp" #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" +#define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt" using namespace std; using namespace time_literals; @@ -124,6 +125,38 @@ Replay::setupReplayFile(const char *file_name) _replay_file = strdup(file_name); } +void +Replay::setParameter(const string ¶meter_name, const double parameter_value) +{ + param_t handle = param_find(parameter_name.c_str()); + param_type_t param_format = param_type(handle); + + if (param_format == PARAM_TYPE_INT32) { + int32_t orig_value = 0; + param_get(handle, &orig_value); + + int32_t value = (int32_t)parameter_value; + + if (orig_value != value) { + PX4_WARN("Setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); + } + + param_set(handle, (const void *)&value); + + } else if (param_format == PARAM_TYPE_FLOAT) { + float orig_value = 0; + param_get(handle, &orig_value); + + float value = (float)parameter_value; + + if (fabsf(orig_value - value) > FLT_EPSILON) { + PX4_WARN("Setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); + } + + param_set(handle, (const void *)&value); + } +} + void Replay::setUserParams(const char *filename) { @@ -149,39 +182,59 @@ Replay::setUserParams(const char *filename) mystrstream >> pname; mystrstream >> value_string; - double param_value_double = stod(value_string); - - param_t handle = param_find(pname.c_str()); - param_type_t param_format = param_type(handle); _overridden_params.insert(pname); - if (param_format == PARAM_TYPE_INT32) { - int32_t orig_value = 0; - param_get(handle, &orig_value); + double param_value_double = stod(value_string); - int32_t value = (int32_t)param_value_double; - - if (orig_value != value) { - PX4_WARN("setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); - } - - param_set(handle, (const void *)&value); - - } else if (param_format == PARAM_TYPE_FLOAT) { - float orig_value = 0; - param_get(handle, &orig_value); - - float value = (float)param_value_double; - - if (fabsf(orig_value - value) > FLT_EPSILON) { - PX4_WARN("setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); - } - - param_set(handle, (const void *)&value); - } + setParameter(pname, param_value_double); } } +void +Replay::readDynamicParams(const char *filename) +{ + _dynamic_parameter_schedule.clear(); + + string line; + string param_name; + string value_string; + string time_string; + ifstream myfile(filename); + + if (!myfile.is_open()) { + return; + } + + PX4_INFO("Reading dynamic params from %s...", filename); + + while (!myfile.eof()) { + getline(myfile, line); + + if (line.empty() || line[0] == '#') { + continue; + } + + istringstream mystrstream(line); + mystrstream >> param_name; + mystrstream >> value_string; + mystrstream >> time_string; + + _dynamic_parameters.insert(param_name); + + double param_value = stod(value_string); + uint64_t change_timestamp = (uint64_t)(stod(time_string) * 1e6); + + // Construct and store parameter change event + ParameterChangeEvent change_event = {change_timestamp, param_name, param_value}; + _dynamic_parameter_schedule.push_back(change_event); + } + + // Sort by event time + sort(_dynamic_parameter_schedule.begin(), _dynamic_parameter_schedule.end()); + + _next_param_change = 0; +} + bool Replay::readFileHeader(std::ifstream &file) { @@ -611,7 +664,8 @@ Replay::readAndApplyParameter(std::ifstream &file, uint16_t msg_size) string type = key.substr(0, pos); string param_name = key.substr(pos + 1); - if (_overridden_params.find(param_name) != _overridden_params.end()) { + if (_overridden_params.find(param_name) != _overridden_params.end() || + _dynamic_parameters.find(param_name) != _dynamic_parameters.end()) { //this parameter is overridden, so don't apply it return true; } @@ -826,6 +880,7 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file) } setUserParams(PARAMS_OVERRIDE_FILE); + readDynamicParams(DYNAMIC_PARAMS_OVERRIDE_FILE); return true; } @@ -896,7 +951,7 @@ Replay::run() Subscription &sub = *_subscriptions[next_msg_id]; - if (next_file_time == 0) { + if (next_file_time == 0 || next_file_time < _file_start_time) { //someone didn't set the timestamp properly. Consider the message invalid nextDataMessage(replay_file, sub, next_msg_id); continue; @@ -908,6 +963,17 @@ Replay::run() readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); last_additional_message_pos = next_additional_message_pos; + // Perform scheduled parameter changes + while (_next_param_change < _dynamic_parameter_schedule.size() && + _dynamic_parameter_schedule[_next_param_change].timestamp <= next_file_time) { + const auto param_change = _dynamic_parameter_schedule[_next_param_change]; + PX4_WARN("Performing param change scheduled for t=%.3lf at t=%.3lf.", + (double)param_change.timestamp / 1.e6, + (double)next_file_time / 1.e6); + setParameter(param_change.parameter_name, param_change.parameter_value); + _next_param_change++; + } + const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset); // It's time to publish diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 94ecc9d416..ef6f0ee97b 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -220,6 +221,23 @@ protected: private: std::set _overridden_params; + + struct ParameterChangeEvent { + uint64_t timestamp; + std::string parameter_name; + double parameter_value; + + // Comparison operator such that sorting is done by timestamp + bool operator<(const ParameterChangeEvent &other) const + { + return timestamp < other.timestamp; + } + }; + + std::set _dynamic_parameters; + std::vector _dynamic_parameter_schedule; + size_t _next_param_change; + std::map _file_formats; ///< all formats we read from the file uint64_t _file_start_time; @@ -275,7 +293,9 @@ private: /** get the size of a type that can be an array */ static size_t sizeOfFullType(const std::string &type_name_full); + void setParameter(const std::string ¶meter_name, const double parameter_value); void setUserParams(const char *filename); + void readDynamicParams(const char *filename); std::string parseOrbFields(const std::string &fields);