diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay b/ROMFS/px4fmu_common/init.d-posix/rc.replay index cfbd53eb77..f4f0f210f8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rc.replay +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index dc8e5a31ee..677122634d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/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 diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 85850dc6d3..142b04c1b9 100644 --- a/boards/px4/sitl/default.cmake +++ b/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) diff --git a/boards/px4/sitl/replay.cmake b/boards/px4/sitl/replay.cmake index 1cc4219760..df2b6e2181 100644 --- a/boards/px4/sitl/replay.cmake +++ b/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) diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index a6206c6c99..4de85ffe05 100644 --- a/src/modules/replay/Replay.cpp +++ b/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); diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index c5c29565ea..6b52c120ad 100644 --- a/src/modules/replay/Replay.hpp +++ b/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 _subscriptions; std::vector _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 _overridden_params; std::map _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); /** diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 2bea63fd5e..941718323b 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/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 diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 7a29fe3dbf..43c7de1913 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/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