added ekf2 replay module

src/modules/ekf2_replay/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+set(MODULE_CFLAGS)
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
+endif()
+px4_add_module(
+	MODULE modules__ekf2_replay
+	MAIN ekf2_replay
+	COMPILE_FLAGS ${MODULE_CFLAGS}
+	STACK 1000
+	SRCS
+		ekf2_replay_main.cpp
+	DEPENDS
+		platforms__common
+		git_ecl
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -0,0 +1,734 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ekf2_replay_main.cpp
+ * Replay module for ekf2. This module reads ekf2 replay messages from a px4 logfile.
+ * It uses this data to create sensor data for the ekf2 module.
+ *
+ * @author Roman Bapst
+*/
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <float.h>
+
+#include <uORB/topics/ekf2_replay.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/ekf2_innovations.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/control_state.h>
+
+#include <sdlog2/sdlog2_messages.h>
+
+
+extern "C" __EXPORT int ekf2_replay_main(int argc, char *argv[]);
+
+#define PRINT_READ_ERROR 	PX4_WARN("error reading from log file");
+
+// union for log messages to write to log file
+#pragma pack(push, 1)
+struct {
+	uint8_t head1, head2, type;
+	union {
+		struct log_ATT_s att;
+		struct log_LPOS_s lpos;
+		struct log_CTS_s control_state;
+		struct log_EST0_s est0;
+		struct log_EST1_s est1;
+		struct log_EST2_s est2;
+		struct log_EST3_s est3;
+		struct log_EST4_s innov;
+		struct log_EST5_s innov2;
+	} body;
+} log_message;
+
+#pragma pack(pop)
+
+class Ekf2Replay;
+
+
+namespace ekf2_replay
+{
+Ekf2Replay *instance = nullptr;
+}
+
+class Ekf2Replay
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Ekf2Replay(char *logfile);
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~Ekf2Replay();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	void exit() { _task_should_exit = true; }
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	void		task_main();
+
+private:
+	int		_control_task = -1;			/**< task handle for task */
+	bool	_task_should_exit = false;
+
+	orb_advert_t _sensors_pub;
+	orb_advert_t _gps_pub;
+
+	int _att_sub;
+	int _estimator_status_sub;
+	int _innov_sub;
+	int _lpos_sub;
+	int _control_state_sub;
+
+	char *_file_name;
+
+	struct log_format_s _formats[100];
+	struct sensor_combined_s _sensors;
+	struct vehicle_gps_position_s _gps;
+
+	bool _read_part1;
+	bool _read_part2;
+
+	int _write_fd = -1;
+
+	// parse replay message from buffer
+	void parseMessage(uint8_t *source, uint8_t *destination, uint8_t type);
+
+	// copy the replay data from the logs into the topic structs which
+	// will be pushlished after
+	void setSensorData(uint8_t *data, uint8_t type);
+
+	// sensor data for the estimator
+	void publishSensorData();
+
+	void writeMessage(int &fd, void *data, size_t size);
+
+	bool needToSaveMessage(uint8_t type);
+
+	void logIfUpdated();
+};
+
+Ekf2Replay::Ekf2Replay(char *logfile) :
+	_sensors_pub(nullptr),
+	_gps_pub(nullptr),
+	_att_sub(-1),
+	_estimator_status_sub(-1),
+	_innov_sub(-1),
+	_lpos_sub(-1),
+	_control_state_sub(-1),
+	_formats{},
+	_sensors{},
+	_gps{},
+	_read_part1(false),
+	_read_part2(false),
+	_write_fd(-1)
+{
+	// build the path to the log
+	char tmp[] = "./rootfs/";
+	char *path_to_log = (char *) malloc(1 + strlen(tmp) + strlen(logfile));
+	strcpy(path_to_log, tmp);
+	strcat(path_to_log, logfile);
+	_file_name = path_to_log;
+
+}
+
+Ekf2Replay::~Ekf2Replay()
+{
+
+}
+
+void Ekf2Replay::publishSensorData()
+{
+	if (_sensors_pub == nullptr && _read_part1) {
+		_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors);
+
+	} else if (_sensors_pub != nullptr && _read_part1) {
+		orb_publish(ORB_ID(sensor_combined), _sensors_pub, &_sensors);
+	}
+
+	if (_gps_pub == nullptr && _gps.timestamp_position > 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_gps);
+
+	} else if (_gps_pub != nullptr && _gps.timestamp_position > 0) {
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &_gps);
+	}
+}
+
+void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t type)
+{
+	int i = 0;
+	int write_index = 0;
+
+	while (_formats[type].format[i] != '\0') {
+		char data_type = _formats[type].format[i];
+
+		switch (data_type) {
+		case 'f':
+			memcpy(&destination[write_index], &source[write_index], sizeof(float));
+			write_index += sizeof(float);
+			break;
+
+		case 'Q':
+			memcpy(&destination[write_index], &source[write_index], sizeof(uint64_t));
+			write_index += sizeof(uint64_t);
+			break;
+
+		case 'L':
+			memcpy(&destination[write_index], &source[write_index], sizeof(int32_t));
+			write_index += sizeof(int32_t);
+			break;
+
+		case 'M':
+			memcpy(&destination[write_index], &source[write_index], sizeof(uint8_t));
+			write_index += sizeof(uint8_t);
+			break;
+
+		default:
+			PX4_WARN("found unsupported data type in replay message, exiting!");
+			_task_should_exit = true;
+			break;
+		}
+
+		i++;
+	}
+}
+
+void Ekf2Replay::setSensorData(uint8_t *data, uint8_t type)
+{
+	struct log_RPL1_s replay_part1 = {};
+	struct log_RPL2_s replay_part2 = {};
+
+	if (type == LOG_RPL1_MSG) {
+
+		uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref;
+		parseMessage(data, dest_ptr, type);
+		_sensors.timestamp = replay_part1.time_ref;
+		_sensors.gyro_integral_dt[0] = replay_part1.gyro_integral_dt;
+		_sensors.accelerometer_integral_dt[0] = replay_part1.accelerometer_integral_dt;
+		_sensors.magnetometer_timestamp[0] = replay_part1.magnetometer_timestamp;
+		_sensors.baro_timestamp[0] = replay_part1.baro_timestamp;
+		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
+		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
+		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
+		_sensors.accelerometer_integral_m_s[0] = replay_part1.accelerometer_integral_x_m_s;
+		_sensors.accelerometer_integral_m_s[1] = replay_part1.accelerometer_integral_y_m_s;
+		_sensors.accelerometer_integral_m_s[2] = replay_part1.accelerometer_integral_z_m_s;
+		_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
+		_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
+		_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;
+		_sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter;
+		_read_part1 = true;
+
+
+	} else if (type == LOG_RPL2_MSG) {
+		uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec;
+		parseMessage(data, dest_ptr, type);
+		_gps.timestamp_position = replay_part2.time_pos_usec;
+		_gps.timestamp_velocity = replay_part2.time_vel_usec;
+		_gps.lat = replay_part2.lat;
+		_gps.lon = replay_part2.lon;
+		_gps.fix_type = replay_part2.fix_type;
+		_gps.eph = replay_part2.eph;
+		_gps.epv = replay_part2.epv;
+		_gps.vel_m_s = replay_part2.vel_m_s;
+		_gps.vel_n_m_s = replay_part2.vel_n_m_s;
+		_gps.vel_e_m_s = replay_part2.vel_e_m_s;
+		_gps.vel_d_m_s = replay_part2.vel_d_m_s;
+		_gps.vel_ned_valid = replay_part2.vel_ned_valid;
+		_read_part2 = true;
+	}
+}
+
+void Ekf2Replay::writeMessage(int &fd, void *data, size_t size)
+{
+	if (size != ::write(fd, data, size)) {
+		PX4_WARN("error writing to file");
+	}
+}
+
+bool Ekf2Replay::needToSaveMessage(uint8_t type)
+{
+	if (type == LOG_ATT_MSG ||
+	    type == LOG_LPOS_MSG ||
+	    type == LOG_EST0_MSG ||
+	    type == LOG_EST1_MSG ||
+	    type == LOG_EST2_MSG ||
+	    type == LOG_EST3_MSG ||
+	    type == LOG_EST4_MSG ||
+	    type == LOG_EST5_MSG ||
+	    type == LOG_CTS_MSG) {
+		return false;
+	}
+
+	return true;
+}
+
+// update all estimator topics and write them to log file
+void Ekf2Replay::logIfUpdated()
+{
+	bool updated = false;
+
+	// update attitude
+	struct vehicle_attitude_s att = {};
+	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
+	memset(&log_message.body.att.q_w, 0, sizeof(log_ATT_s));
+
+	log_message.type = LOG_ATT_MSG;
+	log_message.head1 = HEAD_BYTE1;
+	log_message.head2 = HEAD_BYTE2;
+	log_message.body.att.q_w = att.q[0];
+	log_message.body.att.q_x = att.q[1];
+	log_message.body.att.q_y = att.q[2];
+	log_message.body.att.q_z = att.q[3];
+	log_message.body.att.roll = att.roll;
+	log_message.body.att.pitch = att.pitch;
+	log_message.body.att.yaw = att.yaw;
+	log_message.body.att.roll_rate = att.rollspeed;
+	log_message.body.att.pitch_rate = att.pitchspeed;
+	log_message.body.att.yaw_rate = att.yawspeed;
+	log_message.body.att.gx = att.g_comp[0];
+	log_message.body.att.gy = att.g_comp[1];
+	log_message.body.att.gz = att.g_comp[2];
+
+	writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length);
+
+	// update local position
+	orb_check(_lpos_sub, &updated);
+
+	if (updated) {
+		struct vehicle_local_position_s lpos = {};
+		orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &lpos);
+
+		log_message.type = LOG_LPOS_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		log_message.body.lpos.x = lpos.x;
+		log_message.body.lpos.y = lpos.y;
+		log_message.body.lpos.z = lpos.z;
+		log_message.body.lpos.ground_dist = lpos.dist_bottom;
+		log_message.body.lpos.ground_dist_rate = lpos.dist_bottom_rate;
+		log_message.body.lpos.vx = lpos.vx;
+		log_message.body.lpos.vy = lpos.vy;
+		log_message.body.lpos.vz = lpos.vz;
+		log_message.body.lpos.ref_lat = lpos.ref_lat * 1e7;
+		log_message.body.lpos.ref_lon = lpos.ref_lon * 1e7;
+		log_message.body.lpos.ref_alt = lpos.ref_alt;
+		log_message.body.lpos.pos_flags = (lpos.xy_valid ? 1 : 0) |
+						  (lpos.z_valid ? 2 : 0) |
+						  (lpos.v_xy_valid ? 4 : 0) |
+						  (lpos.v_z_valid ? 8 : 0) |
+						  (lpos.xy_global ? 16 : 0) |
+						  (lpos.z_global ? 32 : 0);
+		log_message.body.lpos.ground_dist_flags = (lpos.dist_bottom_valid ? 1 : 0);
+		log_message.body.lpos.eph = lpos.eph;
+		log_message.body.lpos.epv = lpos.epv;
+
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_LPOS_MSG].length);
+	}
+
+	// update estimator status
+	orb_check(_estimator_status_sub, &updated);
+
+	if (updated) {
+		struct estimator_status_s est_status = {};
+		orb_copy(ORB_ID(estimator_status), _estimator_status_sub, &est_status);
+		unsigned maxcopy0 = (sizeof(est_status.states) < sizeof(log_message.body.est0.s)) ? sizeof(est_status.states) : sizeof(
+					    log_message.body.est0.s);
+		log_message.type = LOG_EST0_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		memset(&(log_message.body.est0.s), 0, sizeof(log_message.body.est0));
+		memcpy(&(log_message.body.est0.s), est_status.states, maxcopy0);
+		log_message.body.est0.n_states = est_status.n_states;
+		log_message.body.est0.nan_flags = est_status.nan_flags;
+		log_message.body.est0.health_flags = est_status.health_flags;
+		log_message.body.est0.timeout_flags = est_status.timeout_flags;
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST0_MSG].length);
+
+		log_message.type = LOG_EST1_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		unsigned maxcopy1 = ((sizeof(est_status.states) - maxcopy0) < sizeof(log_message.body.est1.s)) ? (sizeof(
+					    est_status.states) - maxcopy0) : sizeof(log_message.body.est1.s);
+		memset(&(log_message.body.est1.s), 0, sizeof(log_message.body.est1.s));
+		memcpy(&(log_message.body.est1.s), ((char *)est_status.states) + maxcopy0, maxcopy1);
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST1_MSG].length);
+
+		log_message.type = LOG_EST2_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		unsigned maxcopy2 = (sizeof(est_status.covariances) < sizeof(log_message.body.est2.cov)) ? sizeof(
+					    est_status.covariances) : sizeof(log_message.body.est2.cov);
+		memset(&(log_message.body.est2.cov), 0, sizeof(log_message.body.est2.cov));
+		memcpy(&(log_message.body.est2.cov), est_status.covariances, maxcopy2);
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST2_MSG].length);
+
+		log_message.type = LOG_EST3_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		unsigned maxcopy3 = ((sizeof(est_status.covariances) - maxcopy2) < sizeof(log_message.body.est3.cov)) ? (sizeof(
+					    est_status.covariances) - maxcopy2) : sizeof(log_message.body.est3.cov);
+		memset(&(log_message.body.est3.cov), 0, sizeof(log_message.body.est3.cov));
+		memcpy(&(log_message.body.est3.cov), ((char *)est_status.covariances) + maxcopy2, maxcopy3);
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST3_MSG].length);
+
+	}
+
+	// update ekf2 innovations
+	orb_check(_innov_sub, &updated);
+
+	if (updated) {
+		struct ekf2_innovations_s innov = {};
+		orb_copy(ORB_ID(ekf2_innovations), _innov_sub, &innov);
+		memset(&log_message.body.innov.s, 0, sizeof(log_message.body.innov.s));
+
+		log_message.type = LOG_EST4_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+
+		for (unsigned i = 0; i < 6; i++) {
+			log_message.body.innov.s[i] = innov.vel_pos_innov[i];
+			log_message.body.innov.s[i + 6] = innov.vel_pos_innov_var[i];
+		}
+
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST4_MSG].length);
+
+		log_message.type = LOG_EST5_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		memset(&(log_message.body.innov2.s), 0, sizeof(log_message.body.innov2.s));
+
+		for (unsigned i = 0; i < 3; i++) {
+			log_message.body.innov2.s[i] = innov.mag_innov[i];
+			log_message.body.innov2.s[i + 3] = innov.mag_innov_var[i];
+		}
+
+		log_message.body.innov2.s[6] = innov.heading_innov;
+		log_message.body.innov2.s[7] = innov.heading_innov_var;
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST5_MSG].length);
+	}
+
+	// update control state
+	orb_check(_control_state_sub, &updated);
+
+	if (updated) {
+		struct control_state_s control_state = {};
+		orb_copy(ORB_ID(control_state), _control_state_sub, &control_state);
+		log_message.type = LOG_CTS_MSG;
+		log_message.head1 = HEAD_BYTE1;
+		log_message.head2 = HEAD_BYTE2;
+		log_message.body.control_state.vx_body = control_state.x_vel;
+		log_message.body.control_state.vy_body = control_state.y_vel;
+		log_message.body.control_state.vz_body = control_state.z_vel;
+		log_message.body.control_state.airspeed = control_state.airspeed;
+		log_message.body.control_state.roll_rate = control_state.roll_rate;
+		log_message.body.control_state.pitch_rate = control_state.pitch_rate;
+		log_message.body.control_state.yaw_rate = control_state.yaw_rate;
+		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_CTS_MSG].length);
+	}
+}
+
+void Ekf2Replay::task_main()
+{
+	// formats
+	const int _k_max_data_size = 1024;	// 16x16 bytes
+	uint8_t data[_k_max_data_size] = {};
+
+	// Open log file from which we read data
+	// TODO Check if file exists
+	int fd = ::open(_file_name, O_RDONLY);
+
+	// replay log file
+	char *replay_log_name;
+	replay_log_name = strtok(_file_name, ".");
+
+	char tmp[] = "_replayed.px4log";
+
+	char *path_to_replay_log = (char *) malloc(1 + strlen(tmp) + strlen(replay_log_name));
+	strcpy(path_to_replay_log, ".");
+	strcat(path_to_replay_log, replay_log_name);
+	strcat(path_to_replay_log, tmp);
+
+	// open logfile to write
+	_write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU);
+
+	// subscribe to estimator topics
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	_innov_sub = orb_subscribe(ORB_ID(ekf2_innovations));
+	_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_control_state_sub = orb_subscribe(ORB_ID(control_state));
+
+	px4_pollfd_struct_t fds[1];
+
+	// we use attitude updates from the estimator for synchronisation
+	fds[0].fd = _att_sub;
+	fds[0].events = POLLIN;
+
+	bool read_first_header = false;
+
+	PX4_WARN("Replay in progress...");
+	PX4_WARN("Log data will be written to %s", path_to_replay_log);
+
+	while (!_task_should_exit) {
+		uint8_t header[3] = {};
+
+		if (::read(fd, header, 3) != 3) {
+			if (!read_first_header) {
+				PX4_WARN("error reading log file, is the path printed above correct?");
+
+			} else {
+				PX4_WARN("Done!");
+			}
+
+			_task_should_exit = true;
+			continue;
+		}
+
+		read_first_header = true;
+
+		if (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2) {
+			PX4_WARN("bad log header\n");
+			_task_should_exit = true;
+			continue;
+		}
+
+		// write header but only for messages which are not generated by the estimator
+		if (needToSaveMessage(header[2])) {
+			writeMessage(_write_fd, &header[0], 3);
+		}
+
+		if (header[2] == LOG_FORMAT_MSG) {
+			// format message
+			struct log_format_s f;
+
+			if (::read(fd, &f.type, sizeof(f)) != sizeof(f)) {
+				PRINT_READ_ERROR;
+				_task_should_exit = true;
+				continue;
+			}
+
+			writeMessage(_write_fd, &f.type, sizeof(log_format_s));
+
+			memcpy(&_formats[f.type], &f, sizeof(f));
+
+		} else if (header[2] == LOG_PARM_MSG) {
+			// parameter message
+			if (::read(fd, &data[0], sizeof(log_PARM_s)) != sizeof(log_PARM_s)) {
+				PRINT_READ_ERROR;
+				_task_should_exit = true;
+				continue;
+			}
+
+			writeMessage(_write_fd, &data[0], sizeof(log_PARM_s));
+
+		} else if (header[2] == LOG_VER_MSG) {
+			// version message
+			if (::read(fd, &data[0], sizeof(log_VER_s)) != sizeof(log_VER_s)) {
+				PRINT_READ_ERROR;
+				_task_should_exit = true;
+				continue;
+			}
+
+			writeMessage(_write_fd, &data[0], sizeof(log_VER_s));
+
+		} else if (header[2] == LOG_TIME_MSG) {
+			// time message
+			if (::read(fd, &data[0], sizeof(log_TIME_s)) != sizeof(log_TIME_s)) {
+				// assume that this is because we have reached the end of the file
+				PX4_WARN("Done!");
+				_task_should_exit = true;
+				continue;
+			}
+
+			writeMessage(_write_fd, &data[0], sizeof(log_TIME_s));
+
+		} else {
+			// data message
+			if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) {
+				PX4_WARN("Done!");
+				_task_should_exit = true;
+				continue;
+			}
+
+			// all messages which we are not getting from the estimator are written
+			// back into the replay log file
+			if (needToSaveMessage(header[2])) {
+				writeMessage(_write_fd, &data[0], _formats[header[2]].length - 3);
+			}
+
+			// set estimator input data
+			setSensorData(&data[0], header[2]);
+
+			// we have read both messages, publish them
+			if (_read_part1 && _read_part2) {
+				publishSensorData();
+
+				// wait for estimator output to arrive
+				int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+
+				if (pret == 0) {
+					PX4_WARN("timeout");
+				}
+
+				if (pret < 0) {
+					PX4_WARN("poll error");
+				}
+
+				if (fds[0].revents & POLLIN) {
+					// write all estimator messages to replay log file
+					logIfUpdated();
+
+					_read_part1 = _read_part2 = false;
+				}
+			}
+		}
+	}
+
+	::close(_write_fd);
+	::close(fd);
+	delete ekf2_replay::instance;
+	ekf2_replay::instance = nullptr;
+}
+
+void Ekf2Replay::task_main_trampoline(int argc, char *argv[])
+{
+	ekf2_replay::instance->task_main();
+}
+
+int Ekf2Replay::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = px4_task_spawn_cmd("ekf2_replay",
+					   SCHED_DEFAULT,
+					   SCHED_PRIORITY_MAX - 5,
+					   3000,
+					   (px4_main_t)&Ekf2Replay::task_main_trampoline,
+					   nullptr);
+
+	if (_control_task < 0) {
+		PX4_WARN("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int ekf2_replay_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		PX4_WARN("usage: ekf2_replay {start|stop|status}");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (ekf2_replay::instance != nullptr) {
+			PX4_WARN("already running");
+			return 1;
+		}
+
+		ekf2_replay::instance = new Ekf2Replay(argv[2]);
+
+		if (ekf2_replay::instance == nullptr) {
+			PX4_WARN("alloc failed");
+			return 1;
+		}
+
+		if (OK != ekf2_replay::instance->start()) {
+			delete ekf2_replay::instance;
+			ekf2_replay::instance = nullptr;
+			PX4_WARN("start failed");
+			return 1;
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (ekf2_replay::instance == nullptr) {
+			PX4_WARN("not running");
+			return 1;
+		}
+
+		ekf2_replay::instance->exit();
+
+		// wait for the destruction of the instance
+		while (ekf2_replay::instance != nullptr) {
+			usleep(50000);
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (ekf2_replay::instance) {
+			PX4_WARN("running");
+			return 0;
+
+		} else {
+			PX4_WARN("not running");
+			return 1;
+		}
+	}
+
+	PX4_WARN("unrecognized command");
+	return 1;
+}