final cleanup

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -100,21 +100,14 @@ Ekf2Replay *instance = nullptr;
 class Ekf2Replay
 {
 public:
-	/**
-	 * Constructor
-	 */
+	// Constructor
 	Ekf2Replay(char *logfile);
 
-	/**
-	 * Destructor, also kills task.
-	 */
+	// Destructor, also kills task
 	~Ekf2Replay();
 
-	/**
-	 * Start task.
-	 *
-	 * @return		OK on success.
-	 */
+	// Start task.
+	// @return		OK on success.
 	int		start();
 
 	void exit() { _task_should_exit = true; }
@@ -124,7 +117,7 @@ public:
 	void		task_main();
 
 private:
-	int		_control_task = -1;			/**< task handle for task */
+	int		_control_task = -1;			//task handle for task
 	bool	_task_should_exit = false;
 
 	orb_advert_t _sensors_pub;
@@ -150,19 +143,32 @@ private:
 	int _write_fd = -1;
 
 	// parse replay message from buffer
+	// @source 			pointer to log message data (excluding header)
+	// @destination 	pointer to message struct of type @type
+	// @type 			message type
 	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();
-
+	// will be puplished after
+	// @data 	pointer to the message struct of type @type
+	// @type 	message type
+	void setEstimatorInput(uint8_t *data, uint8_t type);
+
+	// publish input data for estimator
+	void publishEstimatorInput();
+
+	// write a message to log file
+	// @fd 		file descriptor
+	// @data 	pointer to log message
+	// @data 	size of data to be written
 	void writeMessage(int &fd, void *data, size_t size);
 
+	// determins if we need so write a specific message to the replay log
+	// messages which are not regenerated by the estimator copied from the original log file
+	// @type 	message type
 	bool needToSaveMessage(uint8_t type);
 
+	// get estimator output messages and write them to replay log
 	void logIfUpdated();
 };
 
@@ -192,7 +198,6 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 
 	// we always start landed
 	_status.condition_landed = true;
-
 }
 
 Ekf2Replay::~Ekf2Replay()
@@ -200,21 +205,21 @@ Ekf2Replay::~Ekf2Replay()
 
 }
 
-void Ekf2Replay::publishSensorData()
+void Ekf2Replay::publishEstimatorInput()
 {
-	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);
 	}
+
+	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);
+	}
 }
 
 void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t type)
@@ -261,7 +266,7 @@ void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t typ
 	}
 }
 
-void Ekf2Replay::setSensorData(uint8_t *data, uint8_t type)
+void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 {
 	struct log_RPL1_s replay_part1 = {};
 	struct log_RPL2_s replay_part2 = {};
@@ -519,17 +524,22 @@ void Ekf2Replay::task_main()
 	// TODO Check if file exists
 	int fd = ::open(_file_name, O_RDONLY);
 
-	// replay log file
+	// create path to write a replay 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);
 
+	// create path which tells user location of replay file
+	char tmp2[] = "./build_posix_sitl_replay/src/firmware/posix";
+	char *replay_file_location = (char *) malloc(1 + strlen(tmp) + strlen(tmp2) + strlen(replay_log_name));
+	strcat(replay_file_location, tmp2);
+	strcat(replay_file_location, replay_log_name);
+	strcat(replay_file_location, tmp);
+
 	// open logfile to write
 	_write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU);
 
@@ -548,8 +558,8 @@ void Ekf2Replay::task_main()
 
 	bool read_first_header = false;
 
-	PX4_WARN("Replay in progress...");
-	PX4_WARN("Log data will be written to %s", path_to_replay_log);
+	PX4_INFO("Replay in progress... \n");
+	PX4_INFO("Log data will be written to %s\n", replay_file_location);
 
 	while (!_task_should_exit) {
 		uint8_t header[3] = {};
@@ -559,7 +569,7 @@ void Ekf2Replay::task_main()
 				PX4_WARN("error reading log file, is the path printed above correct?");
 
 			} else {
-				PX4_WARN("Done!");
+				PX4_INFO("Done!");
 			}
 
 			_task_should_exit = true;
@@ -617,7 +627,7 @@ void Ekf2Replay::task_main()
 			// 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!");
+				PX4_INFO("Done!");
 				_task_should_exit = true;
 				continue;
 			}
@@ -627,7 +637,7 @@ void Ekf2Replay::task_main()
 		} else {
 			// data message
 			if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) {
-				PX4_WARN("Done!");
+				PX4_INFO("Done!");
 				_task_should_exit = true;
 				continue;
 			}
@@ -639,11 +649,11 @@ void Ekf2Replay::task_main()
 			}
 
 			// set estimator input data
-			setSensorData(&data[0], header[2]);
+			setEstimatorInput(&data[0], header[2]);
 
 			// we have read both messages, publish them
 			if (_read_part1 && _read_part2) {
-				publishSensorData();
+				publishEstimatorInput();
 
 				// wait for estimator output to arrive
 				int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);