From bdf373c89716d9318a82e7a682ec287421afa133 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 27 Feb 2016 10:07:53 +0100 Subject: [PATCH] final cleanup --- src/modules/ekf2_replay/ekf2_replay_main.cpp | 86 +++++++++++--------- 1 file changed, 48 insertions(+), 38 deletions(-) diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 8efc8c7a33..0958984629 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/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);