diff --git a/msg/ekf2_timestamps.msg b/msg/ekf2_timestamps.msg index 1f8a5f3a0a..1700e827e7 100644 --- a/msg/ekf2_timestamps.msg +++ b/msg/ekf2_timestamps.msg @@ -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 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 241d9bc7d5..1d411de235 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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); - // 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() 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; - - _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() // 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; - - 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); } } diff --git a/src/modules/replay/replay.hpp b/src/modules/replay/replay.hpp index b57e28f277..be20f9db6d 100644 --- a/src/modules/replay/replay.hpp +++ b/src/modules/replay/replay.hpp @@ -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; }; diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index c2cfdd00a2..14eb6435bc 100644 --- a/src/modules/replay/replay_main.cpp +++ b/src/modules/replay/replay_main.cpp @@ -62,13 +62,16 @@ // for ekf2 replay #include #include +#include #include #include -#include +#include +#include #include #include -#include #include +#include +#include #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(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 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;