dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF2 and replay add all consumed messages to ekf2_timestamps and refactor

This commit is contained in:
Daniel Agar 2018-03-19 14:17:16 -04:00
parent 07edec282e
commit 2a58c7a28c
4 changed files with 225 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/ekf2_timestamps.msg
View File

@ -12,13 +12,14 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times
# *_timestamp_rel = absolute timestamp). For int16, this allows a maximum
# difference of +-3.2s to the sensor_combined topic.
int16 airspeed_timestamp_rel
int16 distance_sensor_timestamp_rel
int16 gps_timestamp_rel
int16 optical_flow_timestamp_rel
int16 distance_sensor_timestamp_rel
int16 airspeed_timestamp_rel
int16 vision_position_timestamp_rel
int16 vehicle_air_data_timestamp_rel
int16 vehicle_magnetometer_timestamp_rel
int16 vision_attitude_timestamp_rel
int16 vision_position_timestamp_rel
# Note: this is a high-rate logged topic, so it needs to be as small as possible

371
src/modules/ekf2/ekf2_main.cpp
View File

@ -545,13 +545,7 @@ void Ekf2::run()
// because they will else not always be
// properly populated
sensor_combined_s sensors = {};
vehicle_gps_position_s gps = {};
airspeed_s airspeed = {};
optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {};
vehicle_land_detected_s vehicle_land_detected = {};
vehicle_local_position_s ev_pos = {};
vehicle_attitude_s ev_att = {};
vehicle_status_s vehicle_status = {};
sensor_selection_s sensor_selection = {};
landing_target_pose_s landing_target_pose = {};
@ -584,60 +578,58 @@ void Ekf2::run()
updateParams();
}
bool gps_updated = false;
bool airspeed_updated = false;
bool airdata_updated = false;
bool sensor_selection_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
bool vehicle_land_detected_updated = false;
bool vision_position_updated = false;
bool vision_attitude_updated = false;
bool vehicle_status_updated = false;
bool landing_target_pose_updated = false;
bool magnetometer_updated = false;
orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
// ekf2_timestamps (using 0.1 ms relative timestamps)
ekf2_timestamps_s ekf2_timestamps = {};
ekf2_timestamps.timestamp = sensors.timestamp;
ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
// update all other topics if they have new data
bool vehicle_status_updated = false;
orb_check(status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
if (orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status) == PX4_OK) {
// only fuse synthetic sideslip measurements if conditions are met
_ekf.set_fuse_beta_flag(!vehicle_status.is_rotary_wing && (_fuseBeta.get() == 1));
// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
_ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing);
}
}
orb_check(gps_sub, &gps_updated);
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
}
// Do not attempt to use airspeed if use has been disabled by the user.
orb_check(airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
}
orb_check(airdata_sub, &airdata_updated);
orb_check(magnetometer_sub, &magnetometer_updated);
bool sensor_selection_updated = false;
orb_check(sensor_selection_sub, &sensor_selection_updated);
// Always update sensor selction first time through if time stamp is non zero
if (sensor_selection_updated || (sensor_selection.timestamp == 0)) {
sensor_selection_s sensor_selection_prev = sensor_selection;
orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection);
if ((sensor_selection_prev.timestamp > 0) && (sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
if (sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
PX4_WARN("accel id changed, resetting IMU bias");
imu_bias_reset_request = true;
}
if (orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection) == PX4_OK) {
if ((sensor_selection_prev.timestamp > 0) && (sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
if (sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
PX4_WARN("accel id changed, resetting IMU bias");
imu_bias_reset_request = true;
}
if (sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
PX4_WARN("gyro id changed, resetting IMU bias");
imu_bias_reset_request = true;
if (sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
PX4_WARN("gyro id changed, resetting IMU bias");
imu_bias_reset_request = true;
}
}
}
}
@ -647,47 +639,6 @@ void Ekf2::run()
imu_bias_reset_request = !_ekf.reset_imu_bias();
}
orb_check(optical_flow_sub, &optical_flow_updated);
if (optical_flow_updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
}
if (range_finder_sub_index >= 0) {
orb_check(range_finder_subs[range_finder_sub_index], &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(distance_sensor), range_finder_subs[range_finder_sub_index], &range_finder);
// check if distance sensor is within working boundaries
if (range_finder.min_distance >= range_finder.current_distance ||
range_finder.max_distance <= range_finder.current_distance) {
// use rng_gnd_clearance if on ground
if (_ekf.get_in_air_status()) {
range_finder_updated = false;
} else {
range_finder.current_distance = _rng_gnd_clearance.get();
}
}
}
} else {
range_finder_sub_index = getRangeSubIndex(range_finder_subs);
}
orb_check(ev_pos_sub, &vision_position_updated);
if (vision_position_updated) {
orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
}
orb_check(ev_att_sub, &vision_attitude_updated);
if (vision_attitude_updated) {
orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
}
// in replay mode we are getting the actual timestamp from the sensor topic
hrt_abstime now = 0;
@ -714,8 +665,11 @@ void Ekf2::run()
_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral);
// read mag data
bool magnetometer_updated = false;
orb_check(magnetometer_sub, &magnetometer_updated);
if (magnetometer_updated) {
vehicle_magnetometer_s magnetometer = {};
vehicle_magnetometer_s magnetometer;
if (orb_copy(ORB_ID(vehicle_magnetometer), magnetometer_sub, &magnetometer) == PX4_OK) {
// Reset learned bias parameters if there has been a persistant change in magnetometer ID
@ -776,15 +730,20 @@ void Ekf2::run()
_mag_data_sum[1] = 0.0f;
_mag_data_sum[2] = 0.0f;
}
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
// read baro data
bool airdata_updated = false;
orb_check(airdata_sub, &airdata_updated);
if (airdata_updated) {
vehicle_air_data_s airdata;
if (orb_copy(ORB_ID(vehicle_air_data), airdata_sub, &airdata) == PX4_OK) {
// If the time last used by the EKF is less than specified, then accumulate the
// data and push the average when the specified interval is reached.
_balt_time_sum_ms += airdata.timestamp / 1000;
@ -830,74 +789,138 @@ void Ekf2::run()
_balt_sample_count = 0;
_balt_data_sum = 0.0f;
}
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
// read gps data if available
bool gps_updated = false;
orb_check(gps_sub, &gps_updated);
if (gps_updated) {
struct gps_message gps_msg;
gps_msg.time_usec = gps.timestamp;
gps_msg.lat = gps.lat;
gps_msg.lon = gps.lon;
gps_msg.alt = gps.alt;
gps_msg.fix_type = gps.fix_type;
gps_msg.eph = gps.eph;
gps_msg.epv = gps.epv;
gps_msg.sacc = gps.s_variance_m_s;
gps_msg.vel_m_s = gps.vel_m_s;
gps_msg.vel_ned[0] = gps.vel_n_m_s;
gps_msg.vel_ned[1] = gps.vel_e_m_s;
gps_msg.vel_ned[2] = gps.vel_d_m_s;
gps_msg.vel_ned_valid = gps.vel_ned_valid;
gps_msg.nsats = gps.satellites_used;
//TODO: add gdop to gps topic
gps_msg.gdop = 0.0f;
vehicle_gps_position_s gps;
_ekf.setGpsData(gps.timestamp, &gps_msg);
}
if (orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps) == PX4_OK) {
struct gps_message gps_msg;
gps_msg.time_usec = gps.timestamp;
gps_msg.lat = gps.lat;
gps_msg.lon = gps.lon;
gps_msg.alt = gps.alt;
gps_msg.fix_type = gps.fix_type;
gps_msg.eph = gps.eph;
gps_msg.epv = gps.epv;
gps_msg.sacc = gps.s_variance_m_s;
gps_msg.vel_m_s = gps.vel_m_s;
gps_msg.vel_ned[0] = gps.vel_n_m_s;
gps_msg.vel_ned[1] = gps.vel_e_m_s;
gps_msg.vel_ned[2] = gps.vel_d_m_s;
gps_msg.vel_ned_valid = gps.vel_ned_valid;
gps_msg.nsats = gps.satellites_used;
//TODO: add gdop to gps topic
gps_msg.gdop = 0.0f;
// only set airspeed data if condition for airspeed fusion are met
bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing
&& (_arspFusionThreshold.get() > FLT_EPSILON)
&& (airspeed.true_airspeed_m_s > _arspFusionThreshold.get());
_ekf.setGpsData(gps.timestamp, &gps_msg);
if (fuse_airspeed) {
const float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
}
if (vehicle_status_updated) {
// only fuse synthetic sideslip measurements if conditions are met
bool fuse_beta = !vehicle_status.is_rotary_wing && (_fuseBeta.get() == 1);
_ekf.set_fuse_beta_flag(fuse_beta);
// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
_ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing);
}
if (optical_flow_updated) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
flow.quality = optical_flow.quality;
flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
flow.gyrodata(2) = optical_flow.gyro_z_rate_integral;
flow.dt = optical_flow.integration_timespan;
if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
}
}
if (range_finder_updated) {
_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
bool airspeed_updated = false;
orb_check(airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
airspeed_s airspeed;
if (orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed)) {
// only set airspeed data if condition for airspeed fusion are met
if ((_arspFusionThreshold.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _arspFusionThreshold.get())) {
const float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
}
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
bool optical_flow_updated = false;
orb_check(optical_flow_sub, &optical_flow_updated);
if (optical_flow_updated) {
optical_flow_s optical_flow;
if (orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow) == PX4_OK) {
flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
flow.quality = optical_flow.quality;
flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
flow.gyrodata(2) = optical_flow.gyro_z_rate_integral;
flow.dt = optical_flow.integration_timespan;
if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
}
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
if (range_finder_sub_index >= 0) {
bool range_finder_updated = false;
orb_check(range_finder_subs[range_finder_sub_index], &range_finder_updated);
if (range_finder_updated) {
distance_sensor_s range_finder;
if (orb_copy(ORB_ID(distance_sensor), range_finder_subs[range_finder_sub_index], &range_finder) == PX4_OK) {
// check if distance sensor is within working boundaries
if (range_finder.min_distance >= range_finder.current_distance ||
range_finder.max_distance <= range_finder.current_distance) {
// use rng_gnd_clearance if on ground
if (_ekf.get_in_air_status()) {
range_finder_updated = false;
} else {
range_finder.current_distance = _rng_gnd_clearance.get();
}
}
_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
} else {
range_finder_sub_index = getRangeSubIndex(range_finder_subs);
}
// get external vision data
// if error estimates are unavailable, use parameter defined defaults
bool vision_position_updated = false;
bool vision_attitude_updated = false;
orb_check(ev_pos_sub, &vision_position_updated);
orb_check(ev_att_sub, &vision_attitude_updated);
if (vision_position_updated || vision_attitude_updated) {
// copy both attitude & position if either updated, we need both to fill a single ext_vision_message
vehicle_attitude_s ev_att = {};
orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
vehicle_local_position_s ev_pos = {};
orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
ext_vision_message ev_data;
ev_data.posNED(0) = ev_pos.x;
ev_data.posNED(1) = ev_pos.y;
@ -914,13 +937,20 @@ void Ekf2::run()
// use timestamp from external computer, clocks are synchronized when using MAVROS
_ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
}
ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
bool vehicle_land_detected_updated = false;
orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) {
orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected);
_ekf.set_in_air_status(!vehicle_land_detected.landed);
if (orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) {
_ekf.set_in_air_status(!vehicle_land_detected.landed);
}
}
// use the landing target pose estimate as another source of velocity data
@ -1358,65 +1388,12 @@ void Ekf2::run()
}
}
{
// publish ekf2_timestamps (using 0.1 ms relative timestamps)
ekf2_timestamps_s ekf2_timestamps;
ekf2_timestamps.timestamp = sensors.timestamp;
// publish ekf2_timestamps
if (_ekf2_timestamps_pub == nullptr) {
_ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps);
if (gps_updated) {
// divide individually to get consistent rounding behavior
ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (optical_flow_updated) {
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (range_finder_updated) {
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (airspeed_updated) {
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (vision_position_updated) {
ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (vision_attitude_updated) {
ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
} else {
ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
}
if (_ekf2_timestamps_pub == nullptr) {
_ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps);
} else {
orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps);
}
} else {
orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps);
}
}

10
src/modules/replay/replay.hpp
View File

@ -308,13 +308,15 @@ private:
static constexpr uint16_t msg_id_invalid = 0xffff;
uint16_t _sensors_combined_msg_id = msg_id_invalid;
uint16_t _airspeed_msg_id = msg_id_invalid;
uint16_t _distance_sensor_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 _sensor_combined_msg_id = msg_id_invalid;
uint16_t _vehicle_air_data_msg_id = msg_id_invalid;
uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
uint16_t _vehicle_vision_attitude_msg_id = msg_id_invalid;
uint16_t _vehicle_vision_position_msg_id = msg_id_invalid;
int _topic_counter = 0;
};

67
src/modules/replay/replay_main.cpp
View File

@ -62,13 +62,16 @@
// for ekf2 replay
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_status.h>
#include "replay.hpp"
@ -953,7 +956,13 @@ bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream
void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
{
if (sub.orb_meta == ORB_ID(sensor_combined)) {
_sensors_combined_msg_id = msg_id;
_sensor_combined_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(airspeed)) {
_airspeed_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(vehicle_gps_position)) {
_gps_msg_id = msg_id;
@ -961,17 +970,17 @@ void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
} 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(vehicle_air_data)) {
_vehicle_air_data_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_magnetometer)) {
_vehicle_magnetometer_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_vision_attitude)) {
_vehicle_vision_attitude_msg_id = msg_id;
} else if (sub.orb_meta == ORB_ID(vehicle_vision_position)) {
_vehicle_vision_position_msg_id = msg_id;
}
// the main loop should only handle publication of the following topics, the sensor topics are
@ -989,28 +998,29 @@ bool ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std
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
handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id);
handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id);
handle_sensor_publication(ekf2_timestamps.gps_timestamp_rel, _gps_msg_id);
handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
handle_sensor_publication(ekf2_timestamps.vision_attitude_timestamp_rel, _vehicle_vision_attitude_msg_id);
handle_sensor_publication(ekf2_timestamps.vision_position_timestamp_rel, _vehicle_vision_position_msg_id);
// 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) {
if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) {
if (_sensor_combined_msg_id == msg_id_invalid) {
// subscription not found yet or sensor_combined not contained in log
return false;
} else if (!_subscriptions[_sensors_combined_msg_id].orb_meta) {
} else if (!_subscriptions[_sensor_combined_msg_id].orb_meta) {
return false; // read past end of file
} else {
// 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());
readTopicDataToBuffer(_subscriptions[_sensor_combined_msg_id], replay_file);
publishTopic(_subscriptions[_sensor_combined_msg_id], _read_buffer.data());
}
}
@ -1068,13 +1078,16 @@ void ReplayEkf2::onExitMainLoop()
PX4_INFO("");
PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):");
print_sensor_statistics(_sensors_combined_msg_id, "sensor_combined");
print_sensor_statistics(_airspeed_msg_id, "airspeed");
print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
print_sensor_statistics(_gps_msg_id, "vehicle_gps_position");
print_sensor_statistics(_optical_flow_msg_id, "optical_flow");
print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
print_sensor_statistics(_airspeed_msg_id, "airspeed");
print_sensor_statistics(_vehicle_vision_position_msg_id, "vehicle_vision_position");
print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
print_sensor_statistics(_vehicle_vision_attitude_msg_id, "vehicle_vision_attitude");
print_sensor_statistics(_vehicle_vision_position_msg_id, "vehicle_vision_position");
orb_unsubscribe(_vehicle_attitude_sub);
_vehicle_attitude_sub = -1;