zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF2 and replay add all consumed messages to ekf2_timestamps and refactor

sbg
Daniel Agar 7 years ago
parent
07edec282e
commit
2a58c7a28c
4 changed files with 219 additions and 226 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      msg/ekf2_timestamps.msg
  2. 359
      src/modules/ekf2/ekf2_main.cpp
  3. 10
      src/modules/replay/replay.hpp
  4. 67
      src/modules/replay/replay_main.cpp

9
msg/ekf2_timestamps.msg
Unescape View File

@ -12,13 +12,14 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times @@ -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

359
src/modules/ekf2/ekf2_main.cpp
Unescape View File

@ -545,13 +545,7 @@ void Ekf2::run() @@ -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() @@ -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);
// update all other topics if they have new data
orb_check(status_sub, &vehicle_status_updated);
// ekf2_timestamps (using 0.1 ms relative timestamps)
ekf2_timestamps_s ekf2_timestamps = {};
ekf2_timestamps.timestamp = sensors.timestamp;
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
}
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;
orb_check(gps_sub, &gps_updated);
// update all other topics if they have new data
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
}
bool vehicle_status_updated = false;
// Do not attempt to use airspeed if use has been disabled by the user.
orb_check(airspeed_sub, &airspeed_updated);
orb_check(status_sub, &vehicle_status_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
if (vehicle_status_updated) {
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(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() @@ -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() @@ -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() @@ -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() @@ -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;
_ekf.setGpsData(gps.timestamp, &gps_msg);
vehicle_gps_position_s gps;
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;
_ekf.setGpsData(gps.timestamp, &gps_msg);
ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
}
}
// 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());
bool airspeed_updated = false;
orb_check(airspeed_sub, &airspeed_updated);
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 (airspeed_updated) {
airspeed_s airspeed;
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);
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);
}
// 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);
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) {
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);
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_updated) {
_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
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() @@ -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() @@ -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;
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;
}
// publish ekf2_timestamps
if (_ekf2_timestamps_pub == nullptr) {
_ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps);
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
Unescape View File

@ -308,13 +308,15 @@ private: @@ -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
Unescape View File

@ -62,13 +62,16 @@ @@ -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 @@ -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) @@ -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(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_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 @@ -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() @@ -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;

Write Preview
Loading…
Cancel
Save