Skip to content

Commit

Permalink
replay add all EKF2 consumed messages
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Mar 26, 2018
1 parent 4d97dda commit 06ec82e
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 30 deletions.
14 changes: 10 additions & 4 deletions src/modules/replay/replay.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -308,13 +308,19 @@ class ReplayEkf2 : public Replay

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 _landing_target_pose_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 _sensor_selection_msg_id = msg_id_invalid;
uint16_t _vehicle_air_data_msg_id = msg_id_invalid;
uint16_t _vehicle_land_detected_msg_id = msg_id_invalid;
uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid;
uint16_t _vehicle_status_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;
};
Expand Down
87 changes: 61 additions & 26 deletions src/modules/replay/replay_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,13 +62,17 @@
// 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/sensor_selection.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"

Expand Down Expand Up @@ -953,25 +957,43 @@ 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;

} else if (sub.orb_meta == ORB_ID(landing_target_pose)) {
_landing_target_pose_msg_id = 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(sensor_selection)) {
_sensor_selection_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_air_data)) {
_vehicle_air_data_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_land_detected)) {
_vehicle_land_detected_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_status)) {
_vehicle_status_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
Expand All @@ -989,28 +1011,33 @@ 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.landing_target_pose_timestamp_rel, _landing_target_pose_msg_id);
handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id);
handle_sensor_publication(ekf2_timestamps.sensor_selection_timestamp_rel, _sensor_selection_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_land_detected_timestamp_rel, _vehicle_land_detected_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id);
handle_sensor_publication(ekf2_timestamps.vehicle_status_timestamp_rel, _vehicle_status_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());
}
}

Expand Down Expand Up @@ -1068,13 +1095,21 @@ 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(_landing_target_pose_msg_id, "landing_target_pose");
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(_sensor_selection_msg_id, "sensor_selection");
print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
print_sensor_statistics(_vehicle_land_detected_msg_id, "vehicle_land_detected");
print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
print_sensor_statistics(_vehicle_status_msg_id, "vehicle_status");
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;
Expand Down

0 comments on commit 06ec82e

Please sign in to comment.