diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index bb212935d6..c6ff14e86a 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include @@ -135,6 +136,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _range_pub; orb_advert_t _airspeed_pub; + orb_advert_t _ev_pub; int _att_sub; int _estimator_status_sub; @@ -151,6 +153,7 @@ private: struct optical_flow_s _flow; struct distance_sensor_s _range; struct airspeed_s _airspeed; + struct vision_position_estimate_s _ev; unsigned _message_counter; // counter which will increase with every message read from the log unsigned _part1_counter_ref; // this is the value of _message_counter when the part1 of the replay message is read (imu data) @@ -158,6 +161,7 @@ private: bool _read_part3; bool _read_part4; bool _read_part6; + bool _read_part5; int _write_fd = -1; px4_pollfd_struct_t _fds[1]; @@ -206,6 +210,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _flow_pub(nullptr), _range_pub(nullptr), _airspeed_pub(nullptr), + _ev_pub(nullptr), _att_sub(-1), _estimator_status_sub(-1), _innov_sub(-1), @@ -223,6 +228,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _read_part3(false), _read_part4(false), _read_part6(false), + _read_part5(false), _write_fd(-1) { // build the path to the log @@ -270,6 +276,15 @@ void Ekf2Replay::publishEstimatorInput() _read_part4 = false; + if (_ev_pub == nullptr && _read_part5) { + _ev_pub = orb_advertise(ORB_ID(vision_position_estimate), &_ev); + + } else if (_ev_pub != nullptr && _read_part5) { + orb_publish(ORB_ID(vision_position_estimate), _ev_pub, &_ev); + } + + _read_part5 = false; + if (_sensors_pub == nullptr) { _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors); @@ -347,6 +362,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) struct log_RPL3_s replay_part3 = {}; struct log_RPL4_s replay_part4 = {}; struct log_RPL6_s replay_part6 = {}; + struct log_RPL5_s replay_part5 = {}; struct log_LAND_s vehicle_landed = {}; if (type == LOG_RPL1_MSG) { @@ -409,9 +425,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _range.current_distance = replay_part4.range_to_ground; _read_part4 = true; - } - - else if (type == LOG_RPL6_MSG){ + } else if (type == LOG_RPL6_MSG){ uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec; parseMessage(data, dest_ptr, type); _airspeed.timestamp = replay_part6.time_airs_usec; @@ -422,9 +436,23 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _airspeed.confidence = replay_part6.confidence; _read_part6 = true; - } + } else if (type == LOG_RPL5_MSG) { + uint8_t *dest_ptr = (uint8_t *)&replay_part5.time_ev_usec; + parseMessage(data, dest_ptr, type); + _ev.timestamp = replay_part5.time_ev_usec; + _ev.timestamp_computer = replay_part5.time_ev_usec; // fake this timestamp + _ev.x = replay_part5.x; + _ev.y = replay_part5.y; + _ev.z = replay_part5.z; + _ev.q[0] = replay_part5.q0; + _ev.q[1] = replay_part5.q1; + _ev.q[2] = replay_part5.q2; + _ev.q[3] = replay_part5.q3; + _ev.pos_err = replay_part5.pos_err; + _ev.ang_err = replay_part5.pos_err; + _read_part5 = true; - else if (type == LOG_LAND_MSG) { + } else if (type == LOG_LAND_MSG) { uint8_t *dest_ptr = (uint8_t *)&vehicle_landed.landed; parseMessage(data, dest_ptr, type); _land_detected.landed = vehicle_landed.landed;