From 89ace03133707f2da773223f3c1ca3de78812f96 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 9 Mar 2016 22:46:17 +0100 Subject: [PATCH] ekf2_replay: added support for flow and range data --- src/modules/ekf2_replay/ekf2_replay_main.cpp | 63 ++++++++++++++++++-- 1 file changed, 57 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 570ec3786e..b4f4ecc6bf 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -62,6 +62,8 @@ #include #include #include +#include +#include #include @@ -123,6 +125,8 @@ private: orb_advert_t _sensors_pub; orb_advert_t _gps_pub; orb_advert_t _status_pub; + orb_advert_t _flow_pub; + orb_advert_t _range_pub; int _att_sub; int _estimator_status_sub; @@ -136,10 +140,14 @@ private: struct sensor_combined_s _sensors; struct vehicle_gps_position_s _gps; struct vehicle_status_s _status; + struct optical_flow_s _flow; + struct distance_sensor_s _range; 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) bool _read_part2; // indicates if part 2 of replay message has been read + bool _read_part3; + bool _read_part4; int _write_fd = -1; px4_pollfd_struct_t _fds[1]; @@ -183,6 +191,8 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _sensors_pub(nullptr), _gps_pub(nullptr), _status_pub(nullptr), + _flow_pub(nullptr), + _range_pub(nullptr), _att_sub(-1), _estimator_status_sub(-1), _innov_sub(-1), @@ -192,9 +202,13 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _sensors{}, _gps{}, _status{}, + _flow{}, + _range{}, _message_counter(0), _part1_counter_ref(0), _read_part2(false), + _read_part3(false), + _read_part4(false), _write_fd(-1) { // build the path to the log @@ -224,6 +238,22 @@ void Ekf2Replay::publishEstimatorInput() _read_part2 = false; + if (_flow_pub == nullptr && _read_part3) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &_flow); + } else if (_flow_pub != nullptr && _read_part3) { + orb_publish(ORB_ID(optical_flow), _flow_pub, &_flow); + } + + _read_part3 = false; + + if (_range_pub == nullptr && _read_part4) { + _range_pub = orb_advertise(ORB_ID(distance_sensor), &_range); + } else if (_range_pub != nullptr && _read_part4) { + orb_publish(ORB_ID(distance_sensor), _range_pub, &_range); + } + + _read_part4 = false; + if (_sensors_pub == nullptr) { _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors); @@ -280,6 +310,8 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) { struct log_RPL1_s replay_part1 = {}; struct log_RPL2_s replay_part2 = {}; + struct log_RPL3_s replay_part3 = {}; + struct log_RPL4_s replay_part4 = {}; struct log_STAT_s vehicle_status = {}; if (type == LOG_RPL1_MSG) { @@ -320,6 +352,25 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _gps.vel_ned_valid = replay_part2.vel_ned_valid; _read_part2 = true; + } else if (type == LOG_RPL3_MSG) { + uint8_t *dest_ptr = (uint8_t *)&replay_part3.time_flow_usec; + parseMessage(data, dest_ptr, type); + _flow.timestamp = replay_part3.time_flow_usec; + _flow.pixel_flow_x_integral = replay_part3.flow_integral_x; + _flow.pixel_flow_y_integral = replay_part3.flow_integral_y; + _flow.gyro_x_rate_integral = replay_part3.gyro_integral_x; + _flow.gyro_y_rate_integral = replay_part3.gyro_integral_y; + _flow.integration_timespan = replay_part3.flow_time_integral; + _flow.quality = replay_part3.flow_quality; + _read_part3 = true; + + } else if (type == LOG_RPL4_MSG) { + uint8_t *dest_ptr = (uint8_t *)&replay_part4.time_rng_usec; + parseMessage(data, dest_ptr, type); + _range.timestamp = replay_part4.time_rng_usec; + _range.current_distance = replay_part4.range_to_ground; + _read_part4 = true; + } else if (type == LOG_STAT_MSG) { uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state; parseMessage(data, dest_ptr, type); @@ -681,18 +732,18 @@ void Ekf2Replay::task_main() } if (header[2] == LOG_RPL1_MSG && _part1_counter_ref > 0) { - // we have found two consecutive imu replay messages but haven't published the first one - // so do that now + // we have found another imu replay message while we still have one waiting to be published. + // so publish that now publishAndWaitForEstimator(); } // set estimator input data setEstimatorInput(&data[0], header[2]); - // we have read the imu replay message (part 1) and have waited one more cycle for a potential - // gps replay message (part 2) which is always written direcly after part 1 in the log file - // therefore, we can kick the estimator now - if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter) { + // we have read the imu replay message (part 1) and have waited 3 more cycles for other replay message parts + // e.g. flow, gps or range. we know that in case they were written to the log file they should come right after + // the first replay message, therefore, we can kick the estimator now + if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter - 3) { publishAndWaitForEstimator(); } }