replay: use lockstep scheduler

ROMFS/px4fmu_common/init.d-posix/rc.replay

@@ -16,5 +16,4 @@ param set SDLOG_DIRS_MAX 7
 replay tryapplyparams
 ekf2 start -r
 logger start -f -t -b 1000 -p vehicle_attitude
-sleep 0.2
 replay start

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -240,8 +240,11 @@ then
 fi
 
 dataman start
-replay tryapplyparams
-simulator start -c $simulator_tcp_port
+# only start the simulator if not in replay mode, as both control the lockstep time
+if ! replay tryapplyparams
+then
+	simulator start -c $simulator_tcp_port
+fi
 tone_alarm start
 rc_update start
 sensors start

boards/px4/sitl/default.cmake

@@ -99,9 +99,5 @@ set(REPLAY_FILE "$ENV{replay}")
 if(REPLAY_FILE)
 	message(STATUS "Building with uorb publisher rules support")
 	add_definitions(-DORB_USE_PUBLISHER_RULES)
-
-	message(STATUS "Building without lockstep for replay")
-	set(ENABLE_LOCKSTEP_SCHEDULER no)
-else()
-	set(ENABLE_LOCKSTEP_SCHEDULER yes)
 endif()
+set(ENABLE_LOCKSTEP_SCHEDULER yes)

boards/px4/sitl/replay.cmake

@@ -20,5 +20,4 @@ px4_add_board(
 message(STATUS "Building with uorb publisher rules support")
 add_definitions(-DORB_USE_PUBLISHER_RULES)
 
-message(STATUS "Building without lockstep for replay")
-set(ENABLE_LOCKSTEP_SCHEDULER no)
+set(ENABLE_LOCKSTEP_SCHEDULER yes)

src/modules/replay/Replay.cpp

@@ -67,10 +67,10 @@
 #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt"
 
 using namespace std;
+using namespace time_literals;
 
 namespace px4
 {
-class Replay;
 
 char *Replay::_replay_file = nullptr;
 
@@ -750,6 +750,13 @@ Replay::run()
 		return;
 	}
 
+	_speed_factor = 1.f;
+	const char *speedup = getenv("PX4_SIM_SPEED_FACTOR");
+
+	if (speedup) {
+		_speed_factor = atof(speedup);
+	}
+
 	onEnterMainLoop();
 
 	_replay_start_time = hrt_absolute_time();
@@ -767,9 +774,7 @@ Replay::run()
 		return;
 	}
 
-	//we update the timestamps from the file by a constant offset to match
-	//the current replay time
-	const uint64_t timestamp_offset = _replay_start_time - _file_start_time;
+	const uint64_t timestamp_offset = getTimestampOffset();
 	uint32_t nr_published_messages = 0;
 	streampos last_additional_message_pos = _data_section_start;
 
@@ -849,13 +854,23 @@ Replay::run()
 	if (!should_exit()) {
 		PX4_INFO("Replay done (published %u msgs, %.3lf s)", nr_published_messages,
 			 (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
-
-		//TODO: add parameter -q?
-		replay_file.close();
-		px4_shutdown_request();
 	}
 
 	onExitMainLoop();
+
+	if (!should_exit()) {
+		replay_file.close();
+		px4_shutdown_request();
+		// we need to ensure the shutdown logic gets updated and eventually triggers shutdown
+		hrt_abstime t = hrt_absolute_time();
+
+		for (int i = 0; i < 1000; ++i) {
+			struct timespec ts;
+			abstime_to_ts(&ts, t);
+			px4_clock_settime(CLOCK_MONOTONIC, &ts);
+			t += 10_ms;
+		}
+	}
 }
 
 void
@@ -884,7 +899,20 @@ Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
 
 	// if some topics have a timestamp smaller than the log file start, publish them immediately
 	if (cur_time < publish_timestamp && next_file_time > _file_start_time) {
-		px4_usleep(publish_timestamp - cur_time);
+		if (_speed_factor > FLT_EPSILON) {
+			// avoid many small usleep calls
+			_accumulated_delay += (publish_timestamp - cur_time) / _speed_factor;
+
+			if (_accumulated_delay > 3000) {
+				system_usleep(_accumulated_delay);
+				_accumulated_delay = 0.f;
+			}
+		}
+
+		// adjust the lockstep time to the publication time
+		struct timespec ts;
+		abstime_to_ts(&ts, publish_timestamp);
+		px4_clock_settime(CLOCK_MONOTONIC, &ts);
 	}
 
 	return publish_timestamp;
@@ -987,7 +1015,7 @@ Replay::applyParams(bool quiet)
 {
 	if (!isSetup()) {
 		if (quiet) {
-			return 0;
+			return -1;
 		}
 
 		PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME);

src/modules/replay/Replay.hpp

@@ -206,9 +206,18 @@ protected:
 	 */
 	bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
 
+	virtual uint64_t getTimestampOffset()
+	{
+		//we update the timestamps from the file by a constant offset to match
+		//the current replay time
+		return _replay_start_time - _file_start_time;
+	}
+
 	std::vector<Subscription *> _subscriptions;
 	std::vector<uint8_t> _read_buffer;
 
+	float _speed_factor{1.f}; ///< from PX4_SIM_SPEED_FACTOR env variable (set to 0 to avoid usleep = unlimited rate)
+
 private:
 	std::set<std::string> _overridden_params;
 	std::map<std::string, std::string> _file_formats; ///< all formats we read from the file
@@ -222,6 +231,8 @@ private:
 
 	int64_t _read_until_file_position = 1ULL << 60; ///< read limit if log contains appended data
 
+	float _accumulated_delay{0.f};
+
 	bool readFileHeader(std::ifstream &file);
 
 	/**

src/modules/replay/ReplayEkf2.cpp

@@ -66,32 +66,8 @@ ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &repl
 			return false;
 		}
 
-		px4_pollfd_struct_t fds[1];
-		fds[0].fd = _vehicle_attitude_sub;
-		fds[0].events = POLLIN;
-
-		// wait for a response from the estimator
-		int pret = px4_poll(fds, 1, 1000);
-
-		// introduce some breaks to make sure the logger can keep up
-		if (++_topic_counter == 50) {
-			px4_usleep(1000);
-			_topic_counter = 0;
-		}
-
-		if (pret == 0) {
-			PX4_WARN("poll timeout");
-
-		} else if (pret < 0) {
-			PX4_ERR("poll failed (%i)", pret);
-
-		} else {
-			if (fds[0].revents & POLLIN) {
-				vehicle_attitude_s att;
-				// need to to an orb_copy so that poll will not return immediately
-				orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att);
-			}
-		}
+		// Wait for modules to process the data
+		px4_lockstep_wait_for_components();
 
 		return true;
 
@@ -206,7 +182,7 @@ ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::if
 void
 ReplayEkf2::onEnterMainLoop()
 {
-	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_speed_factor = 0.f; // iterate as fast as possible
 }
 
 void
@@ -233,16 +209,6 @@ ReplayEkf2::onExitMainLoop()
 	print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
 	print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
 	print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");
-
-	orb_unsubscribe(_vehicle_attitude_sub);
-	_vehicle_attitude_sub = -1;
-}
-
-uint64_t
-ReplayEkf2::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
-{
-	// no need for usleep
-	return next_file_time;
 }
 
 } // namespace px4

src/modules/replay/ReplayEkf2.hpp

@@ -50,8 +50,6 @@ protected:
 	void onEnterMainLoop() override;
 	void onExitMainLoop() override;
 
-	uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset) override;
-
 	/**
 	 * handle ekf2 topic publication in ekf2 replay mode
 	 * @param sub
@@ -63,6 +61,11 @@ protected:
 
 	void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override;
 
+	uint64_t getTimestampOffset() override
+	{
+		// avoid offsetting timestamps as we use them to compare against the log
+		return 0;
+	}
 private:
 
 	bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file);
@@ -76,8 +79,6 @@ private:
 	 */
 	bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file);
 
-	int _vehicle_attitude_sub = -1;
-
 	static constexpr uint16_t msg_id_invalid = 0xffff;
 
 	uint16_t _airspeed_msg_id = msg_id_invalid;
@@ -87,8 +88,6 @@ private:
 	uint16_t _vehicle_air_data_msg_id = msg_id_invalid;
 	uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
 	uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid;
-
-	int _topic_counter = 0;
 };
 
 } //namespace px4