forked from Archive/PX4-Autopilot
ekf2 replay: switch from ekf2_replay to ekf2_timestamps topic
This commit is contained in:
Beat Küng
Lorenz Meier
parent
aabdc4125b
commit
2a11a2bc0b
|
|
@ -42,7 +42,7 @@
|
|||
#include "definitions.hpp"
|
||||
|
||||
#include <uORB/uORBTopics.h>
|
||||
#include <uORB/topics/ekf2_replay.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue