diff --git a/msg/ekf2_timestamps.msg b/msg/ekf2_timestamps.msg index f8a7932532..0b4d1c0c87 100644 --- a/msg/ekf2_timestamps.msg +++ b/msg/ekf2_timestamps.msg @@ -15,7 +15,6 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times int16 airspeed_timestamp_rel int16 distance_sensor_timestamp_rel -int16 gps_timestamp_rel int16 optical_flow_timestamp_rel int16 vehicle_air_data_timestamp_rel int16 vehicle_magnetometer_timestamp_rel diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index fb29e7abe8..ac8cf7043d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -804,7 +804,6 @@ void Ekf2::Run() 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; @@ -928,8 +927,6 @@ void Ekf2::Run() if (_gps_subs[0].copy(&gps)) { fillGpsMsgWithVehicleGpsPosData(_gps_state[0], gps); _gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid; - - ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } } diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 4922e56764..2bea63fd5e 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -115,11 +115,6 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t 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)) { - if (sub.multi_id == 0) { - _gps_msg_id = msg_id; - } - } else if (sub.orb_meta == ORB_ID(optical_flow)) { _optical_flow_msg_id = msg_id; @@ -135,9 +130,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) // the main loop should only handle publication of the following topics, the sensor topics are // handled separately in publishEkf2Topics() + // Note: the GPS is not treated here since not missing data is more important than the accuracy of the timestamp sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status) - && sub.orb_meta != ORB_ID(vehicle_land_detected) && - (sub.orb_meta != ORB_ID(vehicle_gps_position) || sub.multi_id == 0); + && sub.orb_meta != ORB_ID(vehicle_land_detected) && sub.orb_meta != ORB_ID(vehicle_gps_position); } bool @@ -153,7 +148,6 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs 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); @@ -234,7 +228,6 @@ ReplayEkf2::onExitMainLoop() 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(_sensor_combined_msg_id, "sensor_combined"); print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data"); diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index bba1ca494c..7a29fe3dbf 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -82,7 +82,6 @@ private: 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 _sensor_combined_msg_id = msg_id_invalid; uint16_t _vehicle_air_data_msg_id = msg_id_invalid;