diff --git a/sysid/src/main/native/cpp/view/DataSelector.cpp b/sysid/src/main/native/cpp/view/DataSelector.cpp index c160490889f..c5bc6a30c7e 100644 --- a/sysid/src/main/native/cpp/view/DataSelector.cpp +++ b/sysid/src/main/native/cpp/view/DataSelector.cpp @@ -155,7 +155,9 @@ DataSelector::Tests DataSelector::LoadTests( std::string_view prevState; Runs* curRuns = nullptr; wpi::log::DataLogReader::iterator lastStart = range.begin(); + int64_t ts = lastStart->GetTimestamp(); for (auto it = range.begin(), end = range.end(); it != end; ++it) { + ts = it->GetTimestamp(); std::string_view testState; if (it->GetEntry() != testStateEntry.entry || !it->GetString(&testState)) { @@ -165,7 +167,7 @@ DataSelector::Tests DataSelector::LoadTests( // track runs as iterator ranges of the same test if (testState != prevState) { if (curRuns) { - curRuns->emplace_back(lastStart, it); + curRuns->emplace_back(lastStart->GetTimestamp(), ts); } lastStart = it; } @@ -189,7 +191,7 @@ DataSelector::Tests DataSelector::LoadTests( } if (curRuns) { - curRuns->emplace_back(lastStart, range.end()); + curRuns->emplace_back(lastStart->GetTimestamp(), ts); } } return tests; @@ -208,34 +210,75 @@ static void AddSample(std::vector>& samples, } else { float val; if (record.GetFloat(&val)) { - samples.emplace_back(units::second_t{record.GetTimestamp() * 1.0e-6}, - T{static_cast(val * scale)}); } } } +template +static void AddSamples(std::vector>& samples, + const std::vector>& data, + int64_t tsbegin, int64_t tsend) { + // data is sorted, so do a binary search for tsbegin and tsend + auto begin = std::lower_bound( + data.begin(), data.end(), tsbegin, + [](const auto& datapoint, double val) { return datapoint.first < val; }); + auto end = std::lower_bound( + begin, data.end(), tsend, + [](const auto& datapoint, double val) { return datapoint.first < val; }); + + for (auto it = begin; it != end; ++it) { + samples.emplace_back(units::second_t{it->first * 1.0e-6}, T{it->second}); + } +} + +static std::vector> GetData( + const glass::DataLogReaderEntry& entry, double scale) { + std::vector> rv; + bool isDouble = entry.type == "double"; + for (auto&& range : entry.ranges) { + for (auto&& record : range) { + if (record.GetEntry() != entry.entry) { + continue; + } + if (isDouble) { + double val; + if (record.GetDouble(&val)) { + rv.emplace_back(record.GetTimestamp(), val * scale); + } + } else { + float val; + if (record.GetFloat(&val)) { + rv.emplace_back(record.GetTimestamp(), + static_cast(val * scale)); + } + } + } + } + + std::sort(rv.begin(), rv.end(), + [](const auto& a, const auto& b) { return a.first < b.first; }); + return rv; +} + TestData DataSelector::BuildTestData() { TestData data; data.distanceUnit = kUnits[m_selectedUnit]; data.mechanismType = analysis::FromName(kAnalysisTypes[m_selectedAnalysis]); - bool voltageDouble = m_voltageEntry->type == "double"; - bool positionDouble = m_positionEntry->type == "double"; - bool velocityDouble = m_velocityEntry->type == "double"; + + // read and sort the entire dataset first; this is memory hungry but + // dramatically speeds up splitting it into runs. + auto voltageData = GetData(*m_voltageEntry, 1.0); + auto positionData = GetData(*m_positionEntry, m_positionScale); + auto velocityData = GetData(*m_velocityEntry, m_velocityScale); for (auto&& test : m_tests) { for (auto&& state : test.second) { auto& motorData = data.motorData[state.first]; - for (auto&& range : state.second) { + for (auto [tsbegin, tsend] : state.second) { auto& run = motorData.runs.emplace_back(); - for (auto&& record : range) { - if (record.GetEntry() == m_voltageEntry->entry) { - AddSample(run.voltage, record, voltageDouble, 1.0); - } else if (record.GetEntry() == m_positionEntry->entry) { - AddSample(run.position, record, positionDouble, m_positionScale); - } else if (record.GetEntry() == m_velocityEntry->entry) { - AddSample(run.velocity, record, velocityDouble, m_velocityScale); - } - } + AddSamples(run.voltage, voltageData, tsbegin, tsend); + AddSamples(run.position, positionData, tsbegin, tsend); + AddSamples(run.velocity, velocityData, tsbegin, tsend); } } } diff --git a/sysid/src/main/native/include/sysid/view/DataSelector.h b/sysid/src/main/native/include/sysid/view/DataSelector.h index 95d7e197d3b..dc4b91d2601 100644 --- a/sysid/src/main/native/include/sysid/view/DataSelector.h +++ b/sysid/src/main/native/include/sysid/view/DataSelector.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -57,7 +58,7 @@ class DataSelector : public glass::View { private: // wpi::Logger& m_logger; - using Runs = std::vector; + using Runs = std::vector>; using State = std::map>; // full name using Tests = std::map>; // e.g. "dynamic" std::future m_testsFuture;