Allow changing parameters during replay (#22071)

* replay: add scheduled parameter changes

* replay: store scheduled parameter change events as structs

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 &parameter_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

src/modules/replay/Replay.hpp

@@ -33,6 +33,7 @@
 
 #pragma once
 
+#include <algorithm>
 #include <fstream>
 #include <map>
 #include <vector>
@@ -220,6 +221,23 @@ protected:
 
 private:
 	std::set<std::string> _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<std::string> _dynamic_parameters;
+	std::vector<ParameterChangeEvent> _dynamic_parameter_schedule;
+	size_t _next_param_change;
+
 	std::map<std::string, std::string> _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 &parameter_name, const double parameter_value);
 	void setUserParams(const char *filename);
+	void readDynamicParams(const char *filename);
 
 	std::string parseOrbFields(const std::string &fields);