From 2a11a2bc0b256eb2c1ab087f6ce128d010ea542f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 3 Mar 2017 08:30:57 +0100 Subject: [PATCH] ekf2 replay: switch from ekf2_replay to ekf2_timestamps topic --- src/modules/replay/replay.hpp | 30 ++-- src/modules/replay/replay_main.cpp | 218 +++++++++++------------------ 2 files changed, 105 insertions(+), 143 deletions(-) diff --git a/src/modules/replay/replay.hpp b/src/modules/replay/replay.hpp index 11dabd9feb..6fec5e0df6 100644 --- a/src/modules/replay/replay.hpp +++ b/src/modules/replay/replay.hpp @@ -42,7 +42,7 @@ #include "definitions.hpp" #include -#include +#include namespace px4 { @@ -231,19 +231,31 @@ protected: */ bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override; + void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override; private: - void publishEkf2Topics(const ekf2_replay_s &ekf2_replay); + bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file); + + /** + * find the next message for a subscription that matches a given timestamp and publish it + * @param timestamp in 0.1 ms + * @param msg_id + * @return true if timestamp found and published + */ + bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file); int _vehicle_attitude_sub = -1; - orb_advert_t _sensors_pub = nullptr; - orb_advert_t _gps_pub = nullptr; - orb_advert_t _flow_pub = nullptr; - orb_advert_t _range_pub = nullptr; - orb_advert_t _airspeed_pub = nullptr; - orb_advert_t _vehicle_vision_position_pub = nullptr; - orb_advert_t _vehicle_vision_attitude_pub = nullptr; + + static constexpr uint16_t msg_id_invalid = 0xffff; + + uint16_t _sensors_combined_msg_id = msg_id_invalid; + uint16_t _gps_msg_id = msg_id_invalid; + uint16_t _optical_flow_msg_id = msg_id_invalid; + uint16_t _distance_sensor_msg_id = msg_id_invalid; + uint16_t _airspeed_msg_id = msg_id_invalid; + uint16_t _vehicle_vision_position_msg_id = msg_id_invalid; + uint16_t _vehicle_vision_attitude_msg_id = msg_id_invalid; int _topic_counter = 0; }; diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index 5895ca7ef9..e954b93304 100644 --- a/src/modules/replay/replay_main.cpp +++ b/src/modules/replay/replay_main.cpp @@ -804,11 +804,13 @@ bool Replay::publishTopic(Subscription &sub, void *data) bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) { - if (sub.orb_meta == ORB_ID(ekf2_replay)) { - struct ekf2_replay_s ekf2_replay; - memcpy(&ekf2_replay, data, sub.orb_meta->o_size); + if (sub.orb_meta == ORB_ID(ekf2_timestamps)) { + ekf2_timestamps_s ekf2_timestamps; + memcpy(&ekf2_timestamps, data, sub.orb_meta->o_size); - publishEkf2Topics(ekf2_replay); + if (!publishEkf2Topics(ekf2_timestamps, replay_file)) { + return false; + } px4_pollfd_struct_t fds[1]; fds[0].fd = _vehicle_attitude_sub; @@ -845,153 +847,101 @@ bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream return false; } -void ReplayEkf2::publishEkf2Topics(const ekf2_replay_s &ekf2_replay) +void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) { - sensor_combined_s sensors; - sensors.timestamp = ekf2_replay.timestamp; - sensors.gyro_integral_dt = ekf2_replay.gyro_integral_dt; - sensors.accelerometer_integral_dt = ekf2_replay.accelerometer_integral_dt; + if (sub.orb_meta == ORB_ID(sensor_combined)) { + _sensors_combined_msg_id = msg_id; - // If the magnetometer timestamp is zero, then there is no valid data - if (ekf2_replay.magnetometer_timestamp == 0) { - sensors.magnetometer_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + } else if (sub.orb_meta == ORB_ID(vehicle_gps_position)) { + _gps_msg_id = msg_id; - } else { - sensors.magnetometer_timestamp_relative = (int32_t)(ekf2_replay.magnetometer_timestamp - sensors.timestamp); + } else if (sub.orb_meta == ORB_ID(optical_flow)) { + _optical_flow_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(distance_sensor)) { + _distance_sensor_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(airspeed)) { + _airspeed_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_vision_position)) { + _vehicle_vision_position_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_vision_attitude)) { + _vehicle_vision_attitude_msg_id = msg_id; } - // If the barometer timestamp is zero then there is no valid data - if (ekf2_replay.baro_timestamp == 0) { - sensors.baro_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + // the main loop should only handle publication of the following topics, the sensor topics are + // handled separately in publishEkf2Topics() + sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status) + && sub.orb_meta != ORB_ID(vehicle_land_detected); +} - } else { - sensors.baro_timestamp_relative = (int32_t)(ekf2_replay.baro_timestamp - sensors.timestamp); - } - - sensors.gyro_rad[0] = ekf2_replay.gyro_rad[0]; - sensors.gyro_rad[1] = ekf2_replay.gyro_rad[1]; - sensors.gyro_rad[2] = ekf2_replay.gyro_rad[2]; - sensors.accelerometer_m_s2[0] = ekf2_replay.accelerometer_m_s2[0]; - sensors.accelerometer_m_s2[1] = ekf2_replay.accelerometer_m_s2[1]; - sensors.accelerometer_m_s2[2] = ekf2_replay.accelerometer_m_s2[2]; - sensors.magnetometer_ga[0] = ekf2_replay.magnetometer_ga[0]; - sensors.magnetometer_ga[1] = ekf2_replay.magnetometer_ga[1]; - sensors.magnetometer_ga[2] = ekf2_replay.magnetometer_ga[2]; - sensors.baro_alt_meter = ekf2_replay.baro_alt_meter; - - if (ekf2_replay.time_usec > 0) { - vehicle_gps_position_s gps; - gps.timestamp = ekf2_replay.time_usec; - gps.lat = ekf2_replay.lat; - gps.lon = ekf2_replay.lon; - gps.fix_type = ekf2_replay.fix_type; - gps.satellites_used = ekf2_replay.nsats; - gps.eph = ekf2_replay.eph; - gps.epv = ekf2_replay.epv; - gps.s_variance_m_s = ekf2_replay.sacc; - gps.vel_m_s = ekf2_replay.vel_m_s; - gps.vel_n_m_s = ekf2_replay.vel_n_m_s; - gps.vel_e_m_s = ekf2_replay.vel_e_m_s; - gps.vel_d_m_s = ekf2_replay.vel_d_m_s; - gps.vel_ned_valid = ekf2_replay.vel_ned_valid; - gps.alt = ekf2_replay.alt; - - if (_gps_pub == nullptr) { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &gps); - - } else { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &gps); +bool ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file) +{ + auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) { + if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) { + uint64_t t = timestamp_relative + ekf2_timestamps.timestamp / 100; // in 0.1 ms + findTimestampAndPublish(t, msg_id, replay_file); } + }; + handle_sensor_publication(ekf2_timestamps.gps_timestamp_rel, _gps_msg_id); // gps + handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); // optical flow + handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); // distance sensor + handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); // airspeed + handle_sensor_publication(ekf2_timestamps.vision_position_timestamp_rel, + _vehicle_vision_position_msg_id); // vision position + handle_sensor_publication(ekf2_timestamps.vision_attitude_timestamp_rel, + _vehicle_vision_attitude_msg_id); // vision attitude - } + // sensor_combined: publish last because ekf2 is polling on this + if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensors_combined_msg_id, replay_file)) { + if (_sensors_combined_msg_id == msg_id_invalid) { + // subscription not found yet or sensor_combined not contained in log + return false; - if (ekf2_replay.flow_timestamp > 0) { - optical_flow_s flow; - flow.timestamp = ekf2_replay.flow_timestamp; - flow.pixel_flow_x_integral = ekf2_replay.flow_pixel_integral[0]; - flow.pixel_flow_y_integral = ekf2_replay.flow_pixel_integral[1]; - flow.gyro_x_rate_integral = ekf2_replay.flow_gyro_integral[0]; - flow.gyro_y_rate_integral = ekf2_replay.flow_gyro_integral[1]; - flow.integration_timespan = ekf2_replay.flow_time_integral; - flow.quality = ekf2_replay.flow_quality; - - if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + } else if (!_subscriptions[_sensors_combined_msg_id].orb_meta) { + return false; // read past end of file } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); + // we should publish a topic, just publish the same again + readTopicDataToBuffer(_subscriptions[_sensors_combined_msg_id], replay_file); + publishTopic(_subscriptions[_sensors_combined_msg_id], _read_buffer.data()); } } - if (ekf2_replay.rng_timestamp > 0) { - distance_sensor_s distance_sensor; - distance_sensor.timestamp = ekf2_replay.rng_timestamp; - distance_sensor.current_distance = ekf2_replay.range_to_ground; - distance_sensor.covariance = 0.0f; - // magic values - distance_sensor.min_distance = 0.05f; - distance_sensor.max_distance = 30.0f; - - if (_range_pub == nullptr) { - _range_pub = orb_advertise(ORB_ID(distance_sensor), &distance_sensor); - - } else { - orb_publish(ORB_ID(distance_sensor), _range_pub, &distance_sensor); - } - } - - if (ekf2_replay.asp_timestamp > 0) { - airspeed_s airspeed; - airspeed.timestamp = ekf2_replay.asp_timestamp; - airspeed.indicated_airspeed_m_s = ekf2_replay.indicated_airspeed_m_s; - airspeed.true_airspeed_m_s = ekf2_replay.true_airspeed_m_s; - - if (_airspeed_pub == nullptr) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } - - if (ekf2_replay.ev_timestamp > 0) { - vehicle_local_position_s vehicle_vision_position{}; - vehicle_attitude_s vehicle_vision_attitude{}; - vehicle_vision_attitude.timestamp = ekf2_replay.ev_timestamp; - vehicle_vision_position.timestamp = ekf2_replay.ev_timestamp; - vehicle_vision_position.x = ekf2_replay.pos_ev[0]; - vehicle_vision_position.y = ekf2_replay.pos_ev[1]; - vehicle_vision_position.z = ekf2_replay.pos_ev[2]; - vehicle_vision_attitude.q[0] = ekf2_replay.quat_ev[0]; - vehicle_vision_attitude.q[1] = ekf2_replay.quat_ev[1]; - vehicle_vision_attitude.q[2] = ekf2_replay.quat_ev[2]; - vehicle_vision_attitude.q[3] = ekf2_replay.quat_ev[3]; - - if (_vehicle_vision_attitude_pub == nullptr) { - _vehicle_vision_attitude_pub = orb_advertise(ORB_ID(vehicle_vision_attitude), &vehicle_vision_attitude); - - } else { - orb_publish(ORB_ID(vehicle_vision_attitude), _vehicle_vision_attitude_pub, &vehicle_vision_attitude); - } - - if (_vehicle_vision_position_pub == nullptr) { - _vehicle_vision_position_pub = orb_advertise(ORB_ID(vehicle_vision_position), &vehicle_vision_position); - - } else { - orb_publish(ORB_ID(vehicle_vision_position), _vehicle_vision_position_pub, &vehicle_vision_position); - } - } - - // publish this last, since ekf2 is polling on this - if (_sensors_pub == nullptr) { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &sensors); - - } else { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &sensors); - } + return true; } +bool ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file) +{ + if (msg_id == msg_id_invalid) { + // could happen if a topic is not logged + return false; + } + + Subscription &sub = _subscriptions[msg_id]; + + while (sub.next_timestamp / 100 < timestamp && sub.orb_meta) { + nextDataMessage(replay_file, sub, msg_id); + } + + if (!sub.orb_meta) { // no messages anymore + return false; + } + + if (sub.next_timestamp / 100 != timestamp) { + // this can happen in beginning of the log or on a dropout + PX4_DEBUG("No timestamp match found for topic %s (%i, %i)", sub.orb_meta->o_name, (int)sub.next_timestamp / 100, + timestamp); + return false; + } + + readTopicDataToBuffer(sub, replay_file); + publishTopic(sub, _read_buffer.data()); + return true; +} void ReplayEkf2::onEnterMainLoop() {