fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(ekf2): remove ranging beacon rate limiting and use local receive time for sample timestamp

Browse Source
This commit is contained in:
Marco Hauswirth 2026-04-08 16:05:11 +02:00
parent c0633d89ff
commit 10c39bab17
2 changed files with 7 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -463,19 +463,11 @@ void EstimatorInterface::setRangingBeaconData(const rangingBeaconSample &ranging
- static_cast<int64_t>(_params.ekf2_rngbc_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost
if (time_us >= static_cast<int64_t>(_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

5
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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<float>(beacon_pos.range) * 1e-3f : NAN;
ranging_beacon.lat = static_cast<double>(beacon_pos.lat) * 1e-7;