diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a6d6ca8038..65e4c938c2 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -463,19 +463,11 @@ void EstimatorInterface::setRangingBeaconData(const rangingBeaconSample &ranging - static_cast(_params.ekf2_rngbc_delay * 1000) - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - // limit data rate to prevent data being lost - if (time_us >= static_cast(_ranging_beacon_buffer->get_newest().time_us + _min_obs_interval_us)) { + rangingBeaconSample ranging_beacon_sample_new{ranging_beacon_sample}; + ranging_beacon_sample_new.time_us = time_us; - rangingBeaconSample ranging_beacon_sample_new{ranging_beacon_sample}; - ranging_beacon_sample_new.time_us = time_us; - - _ranging_beacon_buffer->push(ranging_beacon_sample_new); - _time_last_ranging_beacon_buffer_push = _time_latest_us; - - } else { - ECL_WARN("ranging beacon data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, - _ranging_beacon_buffer->get_newest().time_us, _min_obs_interval_us); - } + _ranging_beacon_buffer->push(ranging_beacon_sample_new); + _time_last_ranging_beacon_buffer_push = _time_latest_us; } #endif // CONFIG_EKF2_RANGING_BEACON diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ce59b940ad..5aca873b63 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2591,8 +2591,9 @@ MavlinkReceiver::handle_message_ranging_beacon(mavlink_message_t *msg) mavlink_msg_ranging_beacon_decode(msg, &beacon_pos); ranging_beacon_s ranging_beacon{}; - ranging_beacon.timestamp = hrt_absolute_time(); - ranging_beacon.timestamp_sample = beacon_pos.time_usec; + const hrt_abstime now = hrt_absolute_time(); + ranging_beacon.timestamp = now; + ranging_beacon.timestamp_sample = now; ranging_beacon.beacon_id = beacon_pos.beacon_id; ranging_beacon.range = (beacon_pos.range != UINT32_MAX) ? static_cast(beacon_pos.range) * 1e-3f : NAN; ranging_beacon.lat = static_cast(beacon_pos.lat) * 1e-7;