Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Allow multiple vision measurements from same timestamp (#4917)

Browse Source 
Co-authored-by: Jordan McMichael <jlmcmchl@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2023-01-12 02:04:30 -05:00
committed by GitHub
parent befd12911c
commit f7f19207e0
13 changed files with 409 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
View File

@@ -68,11 +68,24 @@ class TimeInterpolatableBuffer {
if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
m_pastSnapshots.emplace_back(time, sample);
} else {
m_pastSnapshots.insert(
std::upper_bound(
m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
[](auto t, const auto& pair) { return t < pair.first; }),
std::pair(time, sample));
auto first_after = std::upper_bound(
m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
[](auto t, const auto& pair) { return t < pair.first; });
auto last_not_greater_than = first_after - 1;
if (last_not_greater_than == m_pastSnapshots.begin() ||
last_not_greater_than->first < time) {
// Two cases handled together:
// 1. All entries come after the sample
// 2. Some entries come before the sample, but none are recorded with
// the same time
m_pastSnapshots.insert(first_after, std::pair(time, sample));
} else {
// Final case:
// 3. An entry exists with the same recorded time.
last_not_greater_than->second = sample;
}
}
while (time - m_pastSnapshots[0].first > m_historySize) {
m_pastSnapshots.erase(m_pastSnapshots.begin());