35 #include <px4_platform_common/defines.h> 36 #include <px4_platform_common/posix.h> 69 px4_pollfd_struct_t fds[1];
71 fds[0].events = POLLIN;
83 PX4_WARN(
"poll timeout");
85 }
else if (pret < 0) {
86 PX4_ERR(
"poll failed (%i)", pret);
89 if (fds[0].revents & POLLIN) {
146 auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) {
147 if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) {
149 uint64_t t = timestamp_relative + ekf2_timestamps.
timestamp / 100;
222 auto print_sensor_statistics = [
this](uint16_t msg_id,
const char *
name) {
233 PX4_INFO(
"Topic, Num Published, Num Error (no timestamp match found):");
237 print_sensor_statistics(
_gps_msg_id,
"vehicle_gps_position");
252 return next_file_time;
uint16_t _vehicle_visual_odometry_msg_id
bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file)
find the next message for a subscription that matches a given timestamp and publish it ...
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
void readTopicDataToBuffer(const Subscription &sub, std::ifstream &replay_file)
read a topic from the file (offset given by the subscription) into _read_buffer
bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id)
Find next data message for this subscription, starting with the stored file offset.
bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)
int16_t vehicle_magnetometer_timestamp_rel
std::vector< uint8_t > _read_buffer
High-resolution timer with callouts and timekeeping.
int16_t distance_sensor_timestamp_rel
int16_t airspeed_timestamp_rel
uint16_t _vehicle_magnetometer_msg_id
int orb_subscribe(const struct orb_metadata *meta)
std::vector< Subscription * > _subscriptions
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
bool ignored
if true, it will not be considered for publication in the main loop
void onEnterMainLoop() override
called when entering the main replay loop
int orb_unsubscribe(int handle)
const orb_metadata * orb_meta
if nullptr, this subscription is invalid
bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override
handle ekf2 topic publication in ekf2 replay mode
int16_t gps_timestamp_rel
void onExitMainLoop() override
called when exiting the main replay loop
static constexpr uint16_t msg_id_invalid
uint16_t _vehicle_air_data_msg_id
uint16_t _airspeed_msg_id
int _vehicle_attitude_sub
int16_t optical_flow_timestamp_rel
bool publishTopic(Subscription &sub, void *data)
publish an orb topic
uint16_t _optical_flow_msg_id
uint64_t next_timestamp
timestamp of the file
uint64_t handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset) override
handle delay until topic can be published.
int16_t visual_odometry_timestamp_rel
void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override
called when a new subscription is added
int16_t vehicle_air_data_timestamp_rel
uint16_t _sensor_combined_msg_id
uint16_t _distance_sensor_msg_id