fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 15:43:33 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
+4 -12
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>(_params.ekf2_rngbc_delay * 1000)
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
// limit data rate to prevent data being lost rangingBeaconSample ranging_beacon_sample_new{ranging_beacon_sample};
if (time_us >= static_cast<int64_t>(_ranging_beacon_buffer->get_newest().time_us + _min_obs_interval_us)) { ranging_beacon_sample_new.time_us = time_us;
rangingBeaconSample ranging_beacon_sample_new{ranging_beacon_sample}; _ranging_beacon_buffer->push(ranging_beacon_sample_new);
ranging_beacon_sample_new.time_us = time_us; _time_last_ranging_beacon_buffer_push = _time_latest_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);
}
} }
#endif // CONFIG_EKF2_RANGING_BEACON #endif // CONFIG_EKF2_RANGING_BEACON
src/modules/mavlink/mavlink_receiver.cpp
+3 -2
View File
@@ -2591,8 +2591,9 @@ MavlinkReceiver::handle_message_ranging_beacon(mavlink_message_t *msg)
mavlink_msg_ranging_beacon_decode(msg, &beacon_pos); mavlink_msg_ranging_beacon_decode(msg, &beacon_pos);
ranging_beacon_s ranging_beacon{}; ranging_beacon_s ranging_beacon{};
ranging_beacon.timestamp = hrt_absolute_time(); const hrt_abstime now = hrt_absolute_time();
ranging_beacon.timestamp_sample = beacon_pos.time_usec; ranging_beacon.timestamp = now;
ranging_beacon.timestamp_sample = now;
ranging_beacon.beacon_id = beacon_pos.beacon_id; 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.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; ranging_beacon.lat = static_cast<double>(beacon_pos.lat) * 1e-7;