fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 replay: switch from ekf2_replay to ekf2_timestamps topic

Browse Source
This commit is contained in:
Beat Küng
2017-03-04 22:47:54 +08:00
committed by Lorenz Meier
parent aabdc4125b
commit 2a11a2bc0b
2 changed files with 105 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/replay/replay.hpp
+21 -9
View File
@@ -42,7 +42,7 @@
#include "definitions.hpp" #include "definitions.hpp"
#include <uORB/uORBTopics.h> #include <uORB/uORBTopics.h>
#include <uORB/topics/ekf2_replay.h> #include <uORB/topics/ekf2_timestamps.h>
namespace px4 namespace px4
{ {
@@ -231,19 +231,31 @@ protected:
*/ */
bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override; bool handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) override;
void onSubscriptionAdded(Subscription &sub, uint16_t msg_id) override;
private: 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; int _vehicle_attitude_sub = -1;
orb_advert_t _sensors_pub = nullptr;
orb_advert_t _gps_pub = nullptr; static constexpr uint16_t msg_id_invalid = 0xffff;
orb_advert_t _flow_pub = nullptr;
orb_advert_t _range_pub = nullptr; uint16_t _sensors_combined_msg_id = msg_id_invalid;
orb_advert_t _airspeed_pub = nullptr; uint16_t _gps_msg_id = msg_id_invalid;
orb_advert_t _vehicle_vision_position_pub = nullptr; uint16_t _optical_flow_msg_id = msg_id_invalid;
orb_advert_t _vehicle_vision_attitude_pub = nullptr; 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; int _topic_counter = 0;
}; };
src/modules/replay/replay_main.cpp
+84 -134
View File
@@ -804,11 +804,13 @@ bool Replay::publishTopic(Subscription &sub, void *data)
bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file) bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &replay_file)
{ {
if (sub.orb_meta == ORB_ID(ekf2_replay)) { if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
struct ekf2_replay_s ekf2_replay; ekf2_timestamps_s ekf2_timestamps;
memcpy(&ekf2_replay, data, sub.orb_meta->o_size); 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]; px4_pollfd_struct_t fds[1];
fds[0].fd = _vehicle_attitude_sub; fds[0].fd = _vehicle_attitude_sub;
@@ -845,153 +847,101 @@ bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream
return false; return false;
} }
void ReplayEkf2::publishEkf2Topics(const ekf2_replay_s &ekf2_replay) void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
{ {
sensor_combined_s sensors; if (sub.orb_meta == ORB_ID(sensor_combined)) {
sensors.timestamp = ekf2_replay.timestamp; _sensors_combined_msg_id = msg_id;
sensors.gyro_integral_dt = ekf2_replay.gyro_integral_dt;
sensors.accelerometer_integral_dt = ekf2_replay.accelerometer_integral_dt;
// If the magnetometer timestamp is zero, then there is no valid data } else if (sub.orb_meta == ORB_ID(vehicle_gps_position)) {
if (ekf2_replay.magnetometer_timestamp == 0) { _gps_msg_id = msg_id;
sensors.magnetometer_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
} else { } else if (sub.orb_meta == ORB_ID(optical_flow)) {
sensors.magnetometer_timestamp_relative = (int32_t)(ekf2_replay.magnetometer_timestamp - sensors.timestamp); _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 // the main loop should only handle publication of the following topics, the sensor topics are
if (ekf2_replay.baro_timestamp == 0) { // handled separately in publishEkf2Topics()
sensors.baro_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; 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 { bool ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)
sensors.baro_timestamp_relative = (int32_t)(ekf2_replay.baro_timestamp - sensors.timestamp); {
} auto handle_sensor_publication = [&](int16_t timestamp_relative, uint16_t msg_id) {
if (timestamp_relative != ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID) {
sensors.gyro_rad[0] = ekf2_replay.gyro_rad[0]; uint64_t t = timestamp_relative + ekf2_timestamps.timestamp / 100; // in 0.1 ms
sensors.gyro_rad[1] = ekf2_replay.gyro_rad[1]; findTimestampAndPublish(t, msg_id, replay_file);
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);
} }
};
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) { } else if (!_subscriptions[_sensors_combined_msg_id].orb_meta) {
optical_flow_s flow; return false; // read past end of file
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 { } 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) { return true;
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);
}
} }
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() void ReplayEkf2::onEnterMainLoop()
{ {