From c8d8f24bf0231e23822f3b21daf23b2dd9cb5d19 Mon Sep 17 00:00:00 2001 From: lvpeng Date: Mon, 8 Jun 2026 17:33:16 +0800 Subject: [PATCH] =?UTF-8?q?psd=20=E7=AD=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- XYParser/XYEegParser64.cpp | 55 ++ XYParser/XYEegParser64.h | 45 ++ XYParser/XYEegParserExample.cpp | 62 ++ XYParser/XYImpedanceProcessor.cpp | 216 ++++++ XYParser/XYImpedanceProcessor.h | 34 + XYParser/XYParser.vcxproj | 28 + XYParser/XYParser.vcxproj.filters | 39 ++ XYParser/XYParserApi.cpp | 618 ++++++++++++++++- XYParser/XYParserApi.h | 297 ++++++++- XYParser/XYParserTests/Tests.cpp | 621 +++++++++++++++++- XYParser/XYWelchProcessor.cpp | 285 ++++++++ XYParser/XYWelchProcessor.h | 34 + XYParser/third_party/kissfft/_kiss_fft_guts.h | 156 +++++ XYParser/third_party/kissfft/fir_filter.cpp | 186 ++++++ XYParser/third_party/kissfft/fir_filter.h | 42 ++ XYParser/third_party/kissfft/kiss_fft.c | 349 ++++++++++ XYParser/third_party/kissfft/kiss_fft.h | 93 +++ XYParser/third_party/kissfft/kiss_fft_log.h | 34 + XYParser/third_party/kissfft/kiss_fftr.c | 159 +++++ XYParser/third_party/kissfft/kiss_fftr.h | 32 + .../third_party/kissfft/window_functions.h | 37 ++ 21 files changed, 3408 insertions(+), 14 deletions(-) create mode 100644 XYParser/XYImpedanceProcessor.cpp create mode 100644 XYParser/XYImpedanceProcessor.h create mode 100644 XYParser/XYWelchProcessor.cpp create mode 100644 XYParser/XYWelchProcessor.h create mode 100644 XYParser/third_party/kissfft/_kiss_fft_guts.h create mode 100644 XYParser/third_party/kissfft/fir_filter.cpp create mode 100644 XYParser/third_party/kissfft/fir_filter.h create mode 100644 XYParser/third_party/kissfft/kiss_fft.c create mode 100644 XYParser/third_party/kissfft/kiss_fft.h create mode 100644 XYParser/third_party/kissfft/kiss_fft_log.h create mode 100644 XYParser/third_party/kissfft/kiss_fftr.c create mode 100644 XYParser/third_party/kissfft/kiss_fftr.h create mode 100644 XYParser/third_party/kissfft/window_functions.h diff --git a/XYParser/XYEegParser64.cpp b/XYParser/XYEegParser64.cpp index 30947ec..a886139 100644 --- a/XYParser/XYEegParser64.cpp +++ b/XYParser/XYEegParser64.cpp @@ -1,2 +1,57 @@ #include "pch.h" #include "XYEegParser64.h" + +#include + +namespace { + +constexpr std::uint8_t kFrameHeader = 0xAA; +constexpr std::uint8_t kFrameTail = 0x55; +constexpr std::size_t kReservedByteCount = 6; + +} // namespace + +std::vector XYEegParser64::SerializeImpedanceCommand(ImpedanceSwitch impedance_switch) +{ + std::vector payload(kReservedByteCount, 0); + payload.push_back(static_cast(impedance_switch)); + return SerializeCommand(FunctionCode::kImpedance, payload); +} + +std::vector XYEegParser64::SerializeGainAndSampleRateCommand(GainSwitch gain, + SampleRateSwitch sample_rate) +{ + std::vector payload(kReservedByteCount, 0); + payload.push_back(static_cast(gain)); + payload.push_back(static_cast(sample_rate)); + return SerializeCommand(FunctionCode::kGainSampleRate, payload); +} + +std::vector XYEegParser64::SerializeCommand(FunctionCode function_code, + const std::vector& payload) +{ + std::vector command; + command.reserve(1 + 1 + 2 + payload.size() + 1 + 2); + + command.push_back(kFrameHeader); + command.push_back(static_cast(function_code)); + command.push_back(static_cast((payload.size() >> 8) & 0xFF)); + command.push_back(static_cast(payload.size() & 0xFF)); + command.insert(command.end(), payload.begin(), payload.end()); + command.push_back(Checksum(command, 1, command.size())); + command.push_back(kFrameTail); + command.push_back(kFrameTail); + + return command; +} + +std::uint8_t XYEegParser64::Checksum(const std::vector& bytes, + std::size_t begin_offset, + std::size_t end_offset_exclusive) noexcept +{ + std::uint8_t checksum = 0; + for (std::size_t i = begin_offset; i < end_offset_exclusive && i < bytes.size(); ++i) { + checksum = static_cast(checksum + bytes[i]); + } + return checksum; +} diff --git a/XYParser/XYEegParser64.h b/XYParser/XYEegParser64.h index b9423c5..afa310d 100644 --- a/XYParser/XYEegParser64.h +++ b/XYParser/XYEegParser64.h @@ -7,4 +7,49 @@ using XYEegFrame64 = xyparser::XYEegFrame<64>; class XYEegParser64 final : public xyparser::XYEegTcpParserCommon<64> { public: using Frame = XYEegFrame64; + + enum class FunctionCode : std::uint8_t { + kImpedance = 0x01, + kGainSampleRate = 0x02 + }; + + enum class ImpedanceSwitch : std::uint8_t { + kClose = 0x00, + kOpen = 0x01 + }; + + enum class GainSwitch : std::uint8_t { + k1 = 1, + k2 = 2, + k3 = 3, + k4 = 4, + k6 = 6, + k8 = 8, + k12 = 12, + k24 = 24 + }; + + enum class SampleRateSwitch : std::uint8_t { + k250 = 0, + k500 = 1, + k1000 = 2, + k2000 = 3 + }; + + static constexpr std::size_t kImpedancePayloadSize = 7; + static constexpr std::size_t kGainSampleRatePayloadSize = 8; + static constexpr std::size_t kCommandOverheadSize = 7; + static constexpr std::size_t kImpedanceCommandSize = kImpedancePayloadSize + kCommandOverheadSize; + static constexpr std::size_t kGainSampleRateCommandSize = kGainSampleRatePayloadSize + kCommandOverheadSize; + + static std::vector SerializeImpedanceCommand(ImpedanceSwitch impedance_switch); + static std::vector SerializeGainAndSampleRateCommand(GainSwitch gain, + SampleRateSwitch sample_rate); + +private: + static std::vector SerializeCommand(FunctionCode function_code, + const std::vector& payload); + static std::uint8_t Checksum(const std::vector& bytes, + std::size_t begin_offset, + std::size_t end_offset_exclusive) noexcept; }; diff --git a/XYParser/XYEegParserExample.cpp b/XYParser/XYEegParserExample.cpp index e9efa0a..7dcf022 100644 --- a/XYParser/XYEegParserExample.cpp +++ b/XYParser/XYEegParserExample.cpp @@ -115,3 +115,65 @@ void MinimalExampleFor64ChParser() XYParser_DestroyParser(parser); } + +void MinimalExampleFor64ChControlCommand() +{ + std::array impedance_cmd{}; + std::array setting_cmd{}; + + const int impedance_size = XYParser_Serialize64ImpedanceCommand( + 1, impedance_cmd.data(), impedance_cmd.size()); + const int setting_size = XYParser_Serialize64GainSampleRateCommand( + 24, 1000, setting_cmd.data(), setting_cmd.size()); + + (void)impedance_size; + (void)setting_size; +} + +void MinimalExampleFor8ChImpedanceCommand() +{ + std::array command{}; + const int command_size = XYParser_Serialize8ChImpedanceCommand( + 1, + command.data(), + command.size()); + + if (command_size == static_cast(command.size())) { + const std::uint8_t first_byte = command.front(); + const std::uint8_t payload = command[3]; + (void)first_byte; + (void)payload; + } +} + +void MinimalExampleForImpedanceOutput() +{ + XYParserHandle parser = XYParser_CreateParser(8); + if (parser == nullptr) { + return; + } + + XYParser_SetBypassChecksum(parser, 1); + XYParser_SetSampleRate(parser, 250); + XYParser_SetImpedanceDetection(parser, 1); + + const std::vector raw_bytes = BuildMinimalFrame(8); + std::array frame_summaries{}; + std::array impedance_summaries{}; + + XYParser_Feed(parser, + raw_bytes.data(), + raw_bytes.size(), + frame_summaries.data(), + static_cast(frame_summaries.size())); + + const int impedance_count = XYParser_ReadImpedance(parser, + impedance_summaries.data(), + static_cast(impedance_summaries.size())); + if (impedance_count > 0) { + const std::uint16_t po5_impedance = impedance_summaries.front().impedance_values[LeadChannel_PO5]; + (void)po5_impedance; + } + + XYParser_DestroyParser(parser); +} diff --git a/XYParser/XYImpedanceProcessor.cpp b/XYParser/XYImpedanceProcessor.cpp new file mode 100644 index 0000000..cad4ecf --- /dev/null +++ b/XYParser/XYImpedanceProcessor.cpp @@ -0,0 +1,216 @@ +#include "pch.h" +#include "XYImpedanceProcessor.h" + +#include "third_party/kissfft/kiss_fftr.h" +#include "third_party/kissfft/window_functions.h" + +#include +#include +#include +#include + +namespace { + +constexpr int kDefaultSampleRate = 250; +constexpr int k8ChLeadCount = 8; +constexpr int k64ChLeadCount = 64; + +constexpr std::array k8ChLeadMap = { + LeadChannel_PO5, LeadChannel_POZ, LeadChannel_PO6, LeadChannel_PO7, + LeadChannel_O1, LeadChannel_OZ, LeadChannel_O2, LeadChannel_PO8}; + +constexpr std::array k64ChLeadMap = { + LeadChannel_FP1, LeadChannel_FP2, LeadChannel_PO6, LeadChannel_POZ, + LeadChannel_F3, LeadChannel_F4, LeadChannel_FPZ, LeadChannel_AF4, + LeadChannel_FC3, LeadChannel_PO8, LeadChannel_CP2, LeadChannel_CP1, + LeadChannel_FCZ, LeadChannel_PO5, LeadChannel_FC2, LeadChannel_FC1, + LeadChannel_C3, LeadChannel_C4, LeadChannel_FC4, LeadChannel_CP4, + LeadChannel_P3, LeadChannel_P4, LeadChannel_F5, LeadChannel_C5, + LeadChannel_F6, LeadChannel_PO4, LeadChannel_CP6, LeadChannel_CP5, + LeadChannel_PO3, LeadChannel_CP3, LeadChannel_FC6, LeadChannel_FC5, + LeadChannel_CB1, LeadChannel_CB2, LeadChannel_P5, LeadChannel_AF7, + LeadChannel_A1, LeadChannel_T7, LeadChannel_FT7, LeadChannel_TP7, + LeadChannel_FT8, LeadChannel_AF8, LeadChannel_F8, LeadChannel_F7, + LeadChannel_P6, LeadChannel_C6, LeadChannel_O2, LeadChannel_O1, + LeadChannel_T8, LeadChannel_P7, LeadChannel_CZ, LeadChannel_PZ, + LeadChannel_P8, LeadChannel_FZ, LeadChannel_OZ, LeadChannel_PO7, + LeadChannel_TP8, LeadChannel_AF3, LeadChannel_C2, LeadChannel_C1, + LeadChannel_P2, LeadChannel_P1, LeadChannel_F2, LeadChannel_F1}; + +} // namespace + +XYImpedanceProcessor::XYImpedanceProcessor(std::uint8_t channel_count) + : channel_count_(channel_count) +{ +} + +void XYImpedanceProcessor::SetChannelCount(std::uint8_t channel_count) +{ + channel_count_ = channel_count; + Reset(); +} + +void XYImpedanceProcessor::SetSampleRate(int sample_rate) +{ + if (sample_rate <= 0) { + return; + } + + if (sample_rate_ != sample_rate) { + sample_rate_ = sample_rate; + Reset(); + } +} + +void XYImpedanceProcessor::SetEnabled(bool enabled) +{ + if (enabled_ == enabled) { + return; + } + + enabled_ = enabled; + Reset(); +} + +void XYImpedanceProcessor::Reset() +{ + cached_samples_.clear(); + pending_results_.clear(); +} + +void XYImpedanceProcessor::PushFrame(const XYParserFrameSummary& frame) +{ + if (!enabled_ || frame.channel_count != channel_count_) { + return; + } + + for (std::size_t sample_index = 0; sample_index < frame.sample_count; ++sample_index) { + Sample sample{}; + for (std::size_t channel_index = 0; channel_index < frame.channel_count; ++channel_index) { + sample[channel_index] = frame.channel_values_uv[sample_index][channel_index]; + } + cached_samples_.push_back(sample); + } + + ProcessPendingWindows(); +} + +int XYImpedanceProcessor::Read(XYParserImpedanceSummary* out_summaries, int max_summaries) +{ + if (out_summaries == nullptr || max_summaries <= 0) { + return 0; + } + + const int read_count = std::min(static_cast(pending_results_.size()), max_summaries); + for (int i = 0; i < read_count; ++i) { + out_summaries[i] = pending_results_.front(); + pending_results_.pop_front(); + } + return read_count; +} + +void XYImpedanceProcessor::ProcessPendingWindows() +{ + const int effective_sample_rate = sample_rate_ > 0 ? sample_rate_ : 1; + const std::size_t window_sample_count = static_cast(effective_sample_rate); + while (cached_samples_.size() >= window_sample_count) { + pending_results_.push_back(BuildSummaryFromWindow()); + cached_samples_.erase(cached_samples_.begin(), cached_samples_.begin() + static_cast(window_sample_count)); + } +} + +XYParserImpedanceSummary XYImpedanceProcessor::BuildSummaryFromWindow() const +{ + XYParserImpedanceSummary summary{}; + summary.channel_count = channel_count_; + summary.sample_rate = static_cast(sample_rate_ > 0 ? sample_rate_ : kDefaultSampleRate); + summary.window_sample_count = summary.sample_rate; + + const auto fill_channel = [&](std::size_t channel_index, XYParserLeadChannelNumber lead) { + summary.impedance_values[static_cast(lead)] = EstimateImpedanceValue(channel_index); + }; + + if (channel_count_ == 8) { + for (std::size_t i = 0; i < k8ChLeadMap.size(); ++i) { + fill_channel(i, k8ChLeadMap[i]); + } + return summary; + } + + for (std::size_t i = 0; i < std::min(channel_count_, k64ChLeadMap.size()); ++i) { + fill_channel(i, k64ChLeadMap[i]); + } + return summary; +} + +std::uint16_t XYImpedanceProcessor::EstimateImpedanceValue(std::size_t channel_index) const +{ + const int effective_sample_rate = sample_rate_ > 0 ? sample_rate_ : 1; + const int cached_sample_count = static_cast(cached_samples_.size()); + const int raw_sample_count = cached_sample_count < effective_sample_rate ? cached_sample_count : effective_sample_rate; + if (raw_sample_count < 2 || channel_index >= channel_count_) { + return 0; + } + + const int fft_size = (raw_sample_count % 2 == 0) ? raw_sample_count : (raw_sample_count - 1); + if (fft_size < 2) { + return 0; + } + + kiss_fftr_cfg cfg = kiss_fftr_alloc(fft_size, 0, nullptr, nullptr); + if (cfg == nullptr) { + return 0; + } + + std::vector time_data(static_cast(fft_size), 0.0); + std::vector window(static_cast(fft_size), 0.0); + hanning_function(fft_size, window.data()); + + double window_sum = 0.0; + for (int i = 0; i < fft_size; ++i) { + time_data[static_cast(i)] = cached_samples_[static_cast(i)][channel_index] * window[static_cast(i)]; + window_sum += window[static_cast(i)]; + } + if (window_sum <= 0.0) { + kiss_fftr_free(cfg); + return 0; + } + + std::vector freq_data(static_cast(fft_size / 2 + 1)); + kiss_fftr(cfg, time_data.data(), freq_data.data()); + kiss_fftr_free(cfg); + + const double sample_rate = sample_rate_ > 0 ? static_cast(sample_rate_) : static_cast(kDefaultSampleRate); + const double freq_resolution = sample_rate / static_cast(fft_size); + constexpr double kLineNoiseFreq = 50.0; + constexpr double kLineNoiseHarmonicFreq = 100.0; + constexpr double kRejectHalfBand = 5.0; + constexpr double kImpedanceScale = 0.0008; + + double max_magnitude = 0.0; + for (std::size_t k = 1; k < freq_data.size(); ++k) { + const double frequency = static_cast(k) * freq_resolution; + if (std::abs(frequency - kLineNoiseFreq) <= kRejectHalfBand || + std::abs(frequency - kLineNoiseHarmonicFreq) <= kRejectHalfBand) { + continue; + } + + const double real = freq_data[k].r; + const double imag = freq_data[k].i; + double magnitude = std::sqrt(real * real + imag * imag); + if (k == freq_data.size() - 1 && (fft_size % 2 == 0)) { + magnitude /= window_sum; + } else { + magnitude *= 2.0 / window_sum; + } + + if (magnitude > max_magnitude) { + max_magnitude = magnitude; + } + } + + const double scaled_value = std::round(max_magnitude) * kImpedanceScale; + const long rounded_value = std::lround(scaled_value); + const long clamped_value = rounded_value < 0 ? 0 : (rounded_value > 65535 ? 65535 : rounded_value); + return static_cast(clamped_value); +} diff --git a/XYParser/XYImpedanceProcessor.h b/XYParser/XYImpedanceProcessor.h new file mode 100644 index 0000000..08fbe38 --- /dev/null +++ b/XYParser/XYImpedanceProcessor.h @@ -0,0 +1,34 @@ +#pragma once + +#include "XYParserApi.h" + +#include +#include +#include + +class XYImpedanceProcessor { +public: + explicit XYImpedanceProcessor(std::uint8_t channel_count = 0); + + void SetChannelCount(std::uint8_t channel_count); + void SetSampleRate(int sample_rate); + void SetEnabled(bool enabled); + void Reset(); + + void PushFrame(const XYParserFrameSummary& frame); + int Read(XYParserImpedanceSummary* out_summaries, int max_summaries); + +private: + using Sample = std::array; + + void ProcessPendingWindows(); + XYParserImpedanceSummary BuildSummaryFromWindow() const; + std::uint16_t EstimateImpedanceValue(std::size_t channel_index) const; + +private: + std::uint8_t channel_count_ = 0; + int sample_rate_ = 250; + bool enabled_ = false; + std::deque cached_samples_; + std::deque pending_results_; +}; diff --git a/XYParser/XYParser.vcxproj b/XYParser/XYParser.vcxproj index 36218c8..4d203ae 100644 --- a/XYParser/XYParser.vcxproj +++ b/XYParser/XYParser.vcxproj @@ -137,7 +137,15 @@ + + + + + + + + @@ -150,7 +158,27 @@ Create Create + + NotUsing + NotUsing + NotUsing + NotUsing + + + NotUsing + NotUsing + NotUsing + NotUsing + + + NotUsing + NotUsing + NotUsing + NotUsing + + + diff --git a/XYParser/XYParser.vcxproj.filters b/XYParser/XYParser.vcxproj.filters index 406b3bf..52e7129 100644 --- a/XYParser/XYParser.vcxproj.filters +++ b/XYParser/XYParser.vcxproj.filters @@ -21,9 +21,33 @@ 头文件 + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + + + 头文件 + 头文件 + + 头文件 + 头文件 @@ -41,9 +65,24 @@ 源文件 + + 源文件 + + + 源文件 + + + 源文件 + + + 源文件 + 源文件 + + 源文件 + 源文件 diff --git a/XYParser/XYParserApi.cpp b/XYParser/XYParserApi.cpp index 4b4c35e..17c76bd 100644 --- a/XYParser/XYParserApi.cpp +++ b/XYParser/XYParserApi.cpp @@ -1,23 +1,106 @@ #include "pch.h" #include "XYParserApi.h" +#include "XYImpedanceProcessor.h" +#include "XYWelchProcessor.h" #include "XYEegParser8.h" #include "XYEegParser64.h" #include +#include +#include #include #include #include namespace { +constexpr std::uint8_t kCommandFrameHeader = 0xAA; +constexpr std::uint8_t kCommandFrameTail = 0x55; +constexpr std::size_t k8ChImpedanceCommandSize = 7; +constexpr int k8ChLeadCount = 8; +constexpr int k64ChLeadCount = 64; +constexpr std::uint8_t kAlgorithmChannelCount = 64; +constexpr std::size_t kAlgorithmSampleByteCount = + static_cast(XYPARSER_FRAME_DATA_COLUMN_COUNT) * sizeof(double); + +struct AlgorithmSample { + std::array channel_values_uv{}; + std::uint8_t trigger_type = 0; + std::uint8_t trigger_index = 0; +}; + +std::uint8_t CalculateChecksum(const std::uint8_t* data, std::size_t size) +{ + std::uint8_t checksum = 0; + for (std::size_t i = 0; i < size; ++i) { + checksum = static_cast(checksum + data[i]); + } + return checksum; +} + +std::array Build8ChImpedanceCommand(bool open) +{ + const std::uint8_t payload = open ? XYPARSER_8CH_IMPEDANCE_OPEN : XYPARSER_8CH_IMPEDANCE_CLOSE; + return {kCommandFrameHeader, + 0x00, + 0x01, + payload, + CalculateChecksum(&payload, 1), + kCommandFrameTail, + kCommandFrameTail}; +} + +constexpr std::array k8ChLeadMap = { + LeadChannel_PO5, LeadChannel_POZ, LeadChannel_PO6, LeadChannel_PO7, + LeadChannel_O1, LeadChannel_OZ, LeadChannel_O2, LeadChannel_PO8}; + +constexpr std::array k64ChLeadMap = { + LeadChannel_FP1, LeadChannel_FP2, LeadChannel_PO6, LeadChannel_POZ, + LeadChannel_F3, LeadChannel_F4, LeadChannel_FPZ, LeadChannel_AF4, + LeadChannel_FC3, LeadChannel_PO8, LeadChannel_CP2, LeadChannel_CP1, + LeadChannel_FCZ, LeadChannel_PO5, LeadChannel_FC2, LeadChannel_FC1, + LeadChannel_C3, LeadChannel_C4, LeadChannel_FC4, LeadChannel_CP4, + LeadChannel_P3, LeadChannel_P4, LeadChannel_F5, LeadChannel_C5, + LeadChannel_F6, LeadChannel_PO4, LeadChannel_CP6, LeadChannel_CP5, + LeadChannel_PO3, LeadChannel_CP3, LeadChannel_FC6, LeadChannel_FC5, + LeadChannel_CB1, LeadChannel_CB2, LeadChannel_P5, LeadChannel_AF7, + LeadChannel_A1, LeadChannel_T7, LeadChannel_FT7, LeadChannel_TP7, + LeadChannel_FT8, LeadChannel_AF8, LeadChannel_F8, LeadChannel_F7, + LeadChannel_P6, LeadChannel_C6, LeadChannel_O2, LeadChannel_O1, + LeadChannel_T8, LeadChannel_P7, LeadChannel_CZ, LeadChannel_PZ, + LeadChannel_P8, LeadChannel_FZ, LeadChannel_OZ, LeadChannel_PO7, + LeadChannel_TP8, LeadChannel_AF3, LeadChannel_C2, LeadChannel_C1, + LeadChannel_P2, LeadChannel_P1, LeadChannel_F2, LeadChannel_F1}; + struct ParserContext { std::uint8_t channel_count = 0; XYEegParser8 parser8; XYEegParser64 parser64; + XYImpedanceProcessor impedance_processor; + XYWelchProcessor welch_processor; + double adc_vref = 4.5; + double adc_gain = 6.0; + std::vector algorithm_byte_cache; + std::vector algorithm_sample_cache; + std::uint32_t next_algorithm_frame_index = 1; std::string last_error; }; +struct WelchBandInfo { + const char* name; + double low_hz; + double high_hz; +}; + +constexpr std::array kWelchBandInfos = {{ + {"delta", 0.5, 4.0}, + {"theta", 4.0, 8.0}, + {"alpha", 8.0, 13.0}, + {"beta", 13.0, 30.0}, + {"gamma", 30.0, 50.0}, +}}; + void FillSummary(const XYEegFrame8& frame, XYParserFrameSummary& summary) { summary.frame_index = frame.index; @@ -25,8 +108,8 @@ void FillSummary(const XYEegFrame8& frame, XYParserFrameSummary& summary) summary.battery = frame.battery; summary.sample_count = static_cast(frame.samples.size()); for (std::size_t sample_index = 0; sample_index < XYPARSER_SAMPLES_PER_FRAME; ++sample_index) { - summary.trigger_types[sample_index] = frame.samples[sample_index].trigger_type; - summary.trigger_indices[sample_index] = frame.samples[sample_index].trigger_index; + summary.sample_trigger_types[sample_index] = frame.samples[sample_index].trigger_type; + summary.sample_trigger_indices[sample_index] = frame.samples[sample_index].trigger_index; for (std::size_t channel_index = 0; channel_index < XYPARSER_MAX_CHANNELS; ++channel_index) { summary.channel_values_uv[sample_index][channel_index] = channel_index < frame.channel_count @@ -43,8 +126,8 @@ void FillSummary(const XYEegFrame64& frame, XYParserFrameSummary& summary) summary.battery = frame.battery; summary.sample_count = static_cast(frame.samples.size()); for (std::size_t sample_index = 0; sample_index < XYPARSER_SAMPLES_PER_FRAME; ++sample_index) { - summary.trigger_types[sample_index] = frame.samples[sample_index].trigger_type; - summary.trigger_indices[sample_index] = frame.samples[sample_index].trigger_index; + summary.sample_trigger_types[sample_index] = frame.samples[sample_index].trigger_type; + summary.sample_trigger_indices[sample_index] = frame.samples[sample_index].trigger_index; for (std::size_t channel_index = 0; channel_index < XYPARSER_MAX_CHANNELS; ++channel_index) { summary.channel_values_uv[sample_index][channel_index] = channel_index < frame.channel_count @@ -54,6 +137,143 @@ void FillSummary(const XYEegFrame64& frame, XYParserFrameSummary& summary) } } +void Convert8ChSummaryTo64ChSummary(const XYParserFrameSummary& input_summary, + XYParserFrameSummary& output_summary) +{ + std::memset(&output_summary, 0, sizeof(output_summary)); + output_summary.frame_index = input_summary.frame_index; + output_summary.channel_count = 64; + output_summary.battery = input_summary.battery; + output_summary.sample_count = input_summary.sample_count; + + for (std::size_t sample_index = 0; sample_index < XYPARSER_SAMPLES_PER_FRAME; ++sample_index) { + output_summary.sample_trigger_types[sample_index] = input_summary.sample_trigger_types[sample_index]; + output_summary.sample_trigger_indices[sample_index] = input_summary.sample_trigger_indices[sample_index]; + + for (std::size_t eight_channel_index = 0; eight_channel_index < k8ChLeadMap.size(); ++eight_channel_index) { + const XYParserLeadChannelNumber lead = k8ChLeadMap[eight_channel_index]; + output_summary.channel_values_uv[sample_index][static_cast(lead)] = + input_summary.channel_values_uv[sample_index][eight_channel_index]; + } + } +} + +void Convert64ChSummaryToAlgorithmData(const XYParserFrameSummary& input_summary, + double* output_data) +{ + std::fill(output_data, + output_data + XYPARSER_FRAME_ALGORITHM_VALUE_COUNT, + 0.0); + for (std::size_t sample_index = 0; sample_index < XYPARSER_SAMPLES_PER_FRAME; ++sample_index) { + const std::size_t sample_offset = sample_index * XYPARSER_FRAME_DATA_COLUMN_COUNT; + for (std::size_t channel_index = 0; channel_index < XYPARSER_MAX_CHANNELS; ++channel_index) { + output_data[sample_offset + channel_index] = + input_summary.channel_values_uv[sample_index][channel_index]; + } + output_data[sample_offset + XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX] = + static_cast(input_summary.sample_trigger_types[sample_index]); + output_data[sample_offset + XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX] = + static_cast(input_summary.sample_trigger_indices[sample_index]); + } +} + +void FillSummaryFromAlgorithmSamples(const std::vector& samples, + std::uint32_t frame_index, + XYParserFrameSummary& summary) +{ + std::memset(&summary, 0, sizeof(summary)); + summary.frame_index = frame_index; + summary.channel_count = kAlgorithmChannelCount; + summary.sample_count = static_cast( + std::min(samples.size(), static_cast(XYPARSER_SAMPLES_PER_FRAME))); + + for (std::size_t sample_index = 0; sample_index < samples.size() && + sample_index < static_cast(XYPARSER_SAMPLES_PER_FRAME); + ++sample_index) { + summary.sample_trigger_types[sample_index] = samples[sample_index].trigger_type; + summary.sample_trigger_indices[sample_index] = samples[sample_index].trigger_index; + for (std::size_t channel_index = 0; channel_index < XYPARSER_MAX_CHANNELS; ++channel_index) { + summary.channel_values_uv[sample_index][channel_index] = samples[sample_index].channel_values_uv[channel_index]; + } + } +} + +bool CopyCommandBytes(const std::vector& command, + std::uint8_t* out_buffer, + std::size_t buffer_size) +{ + if (out_buffer == nullptr || buffer_size < command.size()) { + return false; + } + + std::memcpy(out_buffer, command.data(), command.size()); + return true; +} + +bool TryMapSampleRate(std::uint16_t sample_rate, XYEegParser64::SampleRateSwitch& mapped_sample_rate) +{ + switch (sample_rate) { + case 250: + mapped_sample_rate = XYEegParser64::SampleRateSwitch::k250; + return true; + case 500: + mapped_sample_rate = XYEegParser64::SampleRateSwitch::k500; + return true; + case 1000: + mapped_sample_rate = XYEegParser64::SampleRateSwitch::k1000; + return true; + case 2000: + mapped_sample_rate = XYEegParser64::SampleRateSwitch::k2000; + return true; + default: + return false; + } +} + +bool TryMapGain(std::uint8_t gain, XYEegParser64::GainSwitch& mapped_gain) +{ + switch (gain) { + case 1: + mapped_gain = XYEegParser64::GainSwitch::k1; + return true; + case 2: + mapped_gain = XYEegParser64::GainSwitch::k2; + return true; + case 3: + mapped_gain = XYEegParser64::GainSwitch::k3; + return true; + case 4: + mapped_gain = XYEegParser64::GainSwitch::k4; + return true; + case 6: + mapped_gain = XYEegParser64::GainSwitch::k6; + return true; + case 8: + mapped_gain = XYEegParser64::GainSwitch::k8; + return true; + case 12: + mapped_gain = XYEegParser64::GainSwitch::k12; + return true; + case 24: + mapped_gain = XYEegParser64::GainSwitch::k24; + return true; + default: + return false; + } +} + +int GetLeadMapCount(std::uint8_t channel_count) +{ + switch (channel_count) { + case 8: + return k8ChLeadCount; + case 64: + return k64ChLeadCount; + default: + return 0; + } +} + } // namespace extern "C" { @@ -70,6 +290,8 @@ XYParserHandle XYParser_CreateParser(std::uint8_t channel_count) } context->channel_count = channel_count; + context->impedance_processor.SetChannelCount(channel_count); + context->welch_processor.SetChannelCount(channel_count); return context; } @@ -86,6 +308,13 @@ void XYParser_SetAdcParams(XYParserHandle handle, double vref, double gain) return; } + if (vref > 0.0) { + context->adc_vref = vref; + } + if (gain > 0.0) { + context->adc_gain = gain; + } + if (context->channel_count == 8) { context->parser8.SetAdcParams(vref, gain); } else { @@ -108,6 +337,87 @@ void XYParser_SetBypassChecksum(XYParserHandle handle, int bypass) } } +void XYParser_SetSampleRate(XYParserHandle handle, int sample_rate) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr) { + return; + } + + context->impedance_processor.SetSampleRate(sample_rate); + context->welch_processor.SetSampleRate(sample_rate); +} + +void XYParser_SetImpedanceDetection(XYParserHandle handle, int enabled) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr) { + return; + } + + const bool impedance_enabled = enabled != 0; + const double impedance_gain = impedance_enabled ? 24.0 : 6.0; + context->adc_gain = impedance_gain; + + if (context->channel_count == 8) { + context->parser8.SetAdcParams(context->adc_vref, impedance_gain); + } else { + context->parser64.SetAdcParams(context->adc_vref, impedance_gain); + } + + context->impedance_processor.SetEnabled(impedance_enabled); +} + +void XYParser_SetWelchDetection(XYParserHandle handle, int enabled) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr) { + return; + } + + context->welch_processor.SetEnabled(enabled != 0); +} + +std::size_t XYParser_Get64ImpedanceCommandSize(void) +{ + return XYEegParser64::kImpedanceCommandSize; +} + +std::size_t XYParser_Get64GainSampleRateCommandSize(void) +{ + return XYEegParser64::kGainSampleRateCommandSize; +} + +int XYParser_Serialize64ImpedanceCommand(int open, + std::uint8_t* out_buffer, + std::size_t buffer_size) +{ + const auto command = XYEegParser64::SerializeImpedanceCommand( + open != 0 ? XYEegParser64::ImpedanceSwitch::kOpen : XYEegParser64::ImpedanceSwitch::kClose); + if (!CopyCommandBytes(command, out_buffer, buffer_size)) { + return 0; + } + return static_cast(command.size()); +} + +int XYParser_Serialize64GainSampleRateCommand(std::uint8_t gain, + std::uint16_t sample_rate, + std::uint8_t* out_buffer, + std::size_t buffer_size) +{ + XYEegParser64::GainSwitch mapped_gain{}; + XYEegParser64::SampleRateSwitch mapped_sample_rate{}; + if (!TryMapGain(gain, mapped_gain) || !TryMapSampleRate(sample_rate, mapped_sample_rate)) { + return 0; + } + + const auto command = XYEegParser64::SerializeGainAndSampleRateCommand(mapped_gain, mapped_sample_rate); + if (!CopyCommandBytes(command, out_buffer, buffer_size)) { + return 0; + } + return static_cast(command.size()); +} + int XYParser_Feed(XYParserHandle handle, const std::uint8_t* data, std::size_t size, @@ -127,8 +437,13 @@ int XYParser_Feed(XYParserHandle handle, const std::vector frames = context->parser8.Feed(data, size); context->last_error = context->parser8.LastError(); const int write_count = std::min(static_cast(frames.size()), max_summaries); - for (int i = 0; i < write_count; ++i) { - FillSummary(frames[static_cast(i)], out_summaries[i]); + for (std::size_t i = 0; i < frames.size(); ++i) { + XYParserFrameSummary summary{}; + FillSummary(frames[i], summary); + context->impedance_processor.PushFrame(summary); + if (static_cast(i) < write_count) { + out_summaries[static_cast(i)] = summary; + } } return write_count; } @@ -136,12 +451,299 @@ int XYParser_Feed(XYParserHandle handle, const std::vector frames = context->parser64.Feed(data, size); context->last_error = context->parser64.LastError(); const int write_count = std::min(static_cast(frames.size()), max_summaries); - for (int i = 0; i < write_count; ++i) { - FillSummary(frames[static_cast(i)], out_summaries[i]); + for (std::size_t i = 0; i < frames.size(); ++i) { + XYParserFrameSummary summary{}; + FillSummary(frames[i], summary); + context->impedance_processor.PushFrame(summary); + if (static_cast(i) < write_count) { + out_summaries[static_cast(i)] = summary; + } } return write_count; } +int XYParser_Convert8ChFramesTo64Ch(const XYParserFrameSummary* input_summaries, + int input_count, + XYParserFrameSummary* output_summaries, + int max_summaries) +{ + if (input_summaries == nullptr || output_summaries == nullptr || input_count <= 0 || max_summaries <= 0) { + return 0; + } + + const int convert_count = input_count < max_summaries ? input_count : max_summaries; + for (int i = 0; i < convert_count; ++i) { + if (input_summaries[i].channel_count != 8) { + return i; + } + Convert8ChSummaryTo64ChSummary(input_summaries[i], output_summaries[i]); + } + return convert_count; +} + +std::size_t XYParser_GetAlgorithmDataValueCount() +{ + return XYPARSER_FRAME_ALGORITHM_VALUE_COUNT; +} + +int XYParser_ConvertSampleFramesToAlgorithmData(const XYParserFrameSummary* input_summary, + double* output_data) +{ + if (input_summary == nullptr || output_data == nullptr) { + return 0; + } + if (input_summary->channel_count != 64) { + return 0; + } + + Convert64ChSummaryToAlgorithmData(*input_summary, output_data); + return 1; +} + +int XYParser_FeedAlgorithmData(XYParserHandle handle, + const std::uint8_t* data, + std::size_t size, + XYParserFrameSummary* out_summaries, + int max_summaries) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr || context->channel_count != kAlgorithmChannelCount || + data == nullptr || size == 0) { + return 0; + } + + if (max_summaries < 0) { + max_summaries = 0; + } + if (max_summaries > 0 && out_summaries == nullptr) { + return 0; + } + + context->algorithm_byte_cache.insert(context->algorithm_byte_cache.end(), data, data + size); + + while (context->algorithm_byte_cache.size() >= kAlgorithmSampleByteCount) { + AlgorithmSample sample{}; + std::array row{}; + std::memcpy(row.data(), context->algorithm_byte_cache.data(), kAlgorithmSampleByteCount); + for (std::size_t channel_index = 0; channel_index < XYPARSER_MAX_CHANNELS; ++channel_index) { + sample.channel_values_uv[channel_index] = row[channel_index]; + } + sample.trigger_type = static_cast(row[XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX]); + sample.trigger_index = static_cast(row[XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX]); + context->welch_processor.PushSample(sample.channel_values_uv); + context->algorithm_sample_cache.push_back(sample); + context->algorithm_byte_cache.erase( + context->algorithm_byte_cache.begin(), + context->algorithm_byte_cache.begin() + static_cast(kAlgorithmSampleByteCount)); + } + + int output_count = 0; + while (context->algorithm_sample_cache.size() >= static_cast(XYPARSER_SAMPLES_PER_FRAME)) { + const std::vector frame_samples( + context->algorithm_sample_cache.begin(), + context->algorithm_sample_cache.begin() + XYPARSER_SAMPLES_PER_FRAME); + XYParserFrameSummary summary{}; + FillSummaryFromAlgorithmSamples(frame_samples, context->next_algorithm_frame_index++, summary); + if (output_count < max_summaries) { + out_summaries[output_count] = summary; + ++output_count; + } + context->algorithm_sample_cache.erase( + context->algorithm_sample_cache.begin(), + context->algorithm_sample_cache.begin() + XYPARSER_SAMPLES_PER_FRAME); + } + + return output_count; +} + +void XYParser_ResetAlgorithmDataCache(XYParserHandle handle) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr || context->channel_count != kAlgorithmChannelCount) { + return; + } + + context->algorithm_byte_cache.clear(); + context->algorithm_sample_cache.clear(); + context->next_algorithm_frame_index = 1; +} + +int XYParser_FlushAlgorithmData(XYParserHandle handle, + XYParserFrameSummary* out_summary) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr || context->channel_count != kAlgorithmChannelCount || + out_summary == nullptr || context->algorithm_sample_cache.empty()) { + return 0; + } + + FillSummaryFromAlgorithmSamples( + context->algorithm_sample_cache, + context->next_algorithm_frame_index++, + *out_summary); + context->algorithm_sample_cache.clear(); + return 1; +} + +int XYParser_GetLeadMapCount(std::uint8_t channel_count) +{ + return GetLeadMapCount(channel_count); +} + +int XYParser_GetLeadMap(std::uint8_t channel_count, + XYParserLeadChannelNumber* out_leads, + int max_leads) +{ + const int required_count = GetLeadMapCount(channel_count); + if (required_count <= 0 || out_leads == nullptr || max_leads < required_count) { + return 0; + } + + if (channel_count == 8) { + std::copy(k8ChLeadMap.begin(), k8ChLeadMap.end(), out_leads); + return required_count; + } + + std::copy(k64ChLeadMap.begin(), k64ChLeadMap.end(), out_leads); + return required_count; +} + +const char* XYParser_GetLeadName(XYParserLeadChannelNumber lead) +{ + switch (lead) { + case LeadChannel_FP1: return "FP1"; + case LeadChannel_FP2: return "FP2"; + case LeadChannel_PO6: return "PO6"; + case LeadChannel_POZ: return "POZ"; + case LeadChannel_F3: return "F3"; + case LeadChannel_F4: return "F4"; + case LeadChannel_FPZ: return "FPZ"; + case LeadChannel_AF4: return "AF4"; + case LeadChannel_FC3: return "FC3"; + case LeadChannel_PO8: return "PO8"; + case LeadChannel_CP2: return "CP2"; + case LeadChannel_CP1: return "CP1"; + case LeadChannel_FCZ: return "FCZ"; + case LeadChannel_PO5: return "PO5"; + case LeadChannel_FC2: return "FC2"; + case LeadChannel_FC1: return "FC1"; + case LeadChannel_C3: return "C3"; + case LeadChannel_C4: return "C4"; + case LeadChannel_FC4: return "FC4"; + case LeadChannel_CP4: return "CP4"; + case LeadChannel_P3: return "P3"; + case LeadChannel_P4: return "P4"; + case LeadChannel_F5: return "F5"; + case LeadChannel_C5: return "C5"; + case LeadChannel_F6: return "F6"; + case LeadChannel_PO4: return "PO4"; + case LeadChannel_CP6: return "CP6"; + case LeadChannel_CP5: return "CP5"; + case LeadChannel_PO3: return "PO3"; + case LeadChannel_CP3: return "CP3"; + case LeadChannel_FC6: return "FC6"; + case LeadChannel_FC5: return "FC5"; + case LeadChannel_CB1: return "CB1"; + case LeadChannel_CB2: return "CB2"; + case LeadChannel_P5: return "P5"; + case LeadChannel_AF7: return "AF7"; + case LeadChannel_A1: return "A1"; + case LeadChannel_T7: return "T7"; + case LeadChannel_FT7: return "FT7"; + case LeadChannel_TP7: return "TP7"; + case LeadChannel_FT8: return "FT8"; + case LeadChannel_AF8: return "AF8"; + case LeadChannel_F8: return "F8"; + case LeadChannel_F7: return "F7"; + case LeadChannel_P6: return "P6"; + case LeadChannel_C6: return "C6"; + case LeadChannel_O2: return "O2"; + case LeadChannel_O1: return "O1"; + case LeadChannel_T8: return "T8"; + case LeadChannel_P7: return "P7"; + case LeadChannel_CZ: return "CZ"; + case LeadChannel_PZ: return "PZ"; + case LeadChannel_P8: return "P8"; + case LeadChannel_FZ: return "FZ"; + case LeadChannel_OZ: return "OZ"; + case LeadChannel_PO7: return "PO7"; + case LeadChannel_TP8: return "TP8"; + case LeadChannel_AF3: return "AF3"; + case LeadChannel_C2: return "C2"; + case LeadChannel_C1: return "C1"; + case LeadChannel_P2: return "P2"; + case LeadChannel_P1: return "P1"; + case LeadChannel_F2: return "F2"; + case LeadChannel_F1: return "F1"; + case LeadChannel_GND: return "GND"; + default: + return ""; + } +} + +const char* XYParser_GetWelchBandName(int band_index) +{ + if (band_index < 0 || band_index >= static_cast(kWelchBandInfos.size())) { + return ""; + } + return kWelchBandInfos[static_cast(band_index)].name; +} + +int XYParser_GetWelchBandRange(int band_index, double* out_low_hz, double* out_high_hz) +{ + if (band_index < 0 || band_index >= static_cast(kWelchBandInfos.size()) || + out_low_hz == nullptr || out_high_hz == nullptr) { + return 0; + } + + const WelchBandInfo& band_info = kWelchBandInfos[static_cast(band_index)]; + *out_low_hz = band_info.low_hz; + *out_high_hz = band_info.high_hz; + return 1; +} + +int XYParser_ReadImpedance(XYParserHandle handle, + XYParserImpedanceSummary* out_summaries, + int max_summaries) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr) { + return 0; + } + + return context->impedance_processor.Read(out_summaries, max_summaries); +} + +int XYParser_ReadWelch(XYParserHandle handle, + XYParserWelchSummary* out_summaries, + int max_summaries) +{ + ParserContext* context = static_cast(handle); + if (context == nullptr) { + return 0; + } + + return context->welch_processor.Read(out_summaries, max_summaries); +} + +std::size_t XYParser_Get8ChImpedanceCommandSize() +{ + return k8ChImpedanceCommandSize; +} + +int XYParser_Serialize8ChImpedanceCommand(int open, + std::uint8_t* out_command, + std::size_t command_size) +{ + if (out_command == nullptr || command_size < k8ChImpedanceCommandSize) { + return 0; + } + + const auto command = Build8ChImpedanceCommand(open != 0); + std::copy(command.begin(), command.end(), out_command); + return static_cast(command.size()); +} + const char* XYParser_GetLastError(XYParserHandle handle) { ParserContext* context = static_cast(handle); diff --git a/XYParser/XYParserApi.h b/XYParser/XYParserApi.h index 972897c..213af9b 100644 --- a/XYParser/XYParserApi.h +++ b/XYParser/XYParserApi.h @@ -1,4 +1,4 @@ -#pragma once +#pragma once #include #include @@ -15,7 +15,87 @@ typedef void* XYParserHandle; enum { XYPARSER_MAX_CHANNELS = 64, - XYPARSER_SAMPLES_PER_FRAME = 5 + XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX = XYPARSER_MAX_CHANNELS, + XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX = XYPARSER_MAX_CHANNELS + 1, + XYPARSER_FRAME_DATA_COLUMN_COUNT = XYPARSER_MAX_CHANNELS + 2, + XYPARSER_SAMPLES_PER_FRAME = 5, + XYPARSER_FRAME_ALGORITHM_VALUE_COUNT = + XYPARSER_SAMPLES_PER_FRAME * XYPARSER_FRAME_DATA_COLUMN_COUNT, + XYPARSER_WELCH_MAX_FREQUENCY_COUNT = 50, + XYPARSER_WELCH_BAND_COUNT = 5 +}; + +enum XYParser8ChImpedanceSwitch : std::uint8_t { + XYPARSER_8CH_IMPEDANCE_CLOSE = 0xA0, + XYPARSER_8CH_IMPEDANCE_OPEN = 0xA1 +}; + +enum XYParserLeadChannelNumber { + LeadChannel_FP1 = 0, + LeadChannel_FP2, + LeadChannel_PO6, + LeadChannel_POZ, + LeadChannel_F3, + LeadChannel_F4, + LeadChannel_FPZ, + LeadChannel_AF4, + LeadChannel_FC3, + LeadChannel_PO8, + LeadChannel_CP2, + LeadChannel_CP1, + LeadChannel_FCZ, + LeadChannel_PO5, + LeadChannel_FC2, + LeadChannel_FC1, + LeadChannel_C3, + LeadChannel_C4, + LeadChannel_FC4, + LeadChannel_CP4, + LeadChannel_P3, + LeadChannel_P4, + LeadChannel_F5, + LeadChannel_C5, + LeadChannel_F6, + LeadChannel_PO4, + LeadChannel_CP6, + LeadChannel_CP5, + LeadChannel_PO3, + LeadChannel_CP3, + LeadChannel_FC6, + LeadChannel_FC5, + LeadChannel_CB1, + LeadChannel_CB2, + LeadChannel_P5, + LeadChannel_AF7, + LeadChannel_A1, + LeadChannel_T7, + LeadChannel_FT7, + LeadChannel_TP7, + LeadChannel_FT8, + LeadChannel_AF8, + LeadChannel_F8, + LeadChannel_F7, + LeadChannel_P6, + LeadChannel_C6, + LeadChannel_O2, + LeadChannel_O1, + LeadChannel_T8, + LeadChannel_P7, + LeadChannel_CZ, + LeadChannel_PZ, + LeadChannel_P8, + LeadChannel_FZ, + LeadChannel_OZ, + LeadChannel_PO7, + LeadChannel_TP8, + LeadChannel_AF3, + LeadChannel_C2, + LeadChannel_C1, + LeadChannel_P2, + LeadChannel_P1, + LeadChannel_F2, + LeadChannel_F1, + LeadChannel_GND }; struct XYParserFrameSummary { @@ -24,19 +104,228 @@ struct XYParserFrameSummary { std::uint8_t battery; std::uint8_t sample_count; double channel_values_uv[XYPARSER_SAMPLES_PER_FRAME][XYPARSER_MAX_CHANNELS]; - std::uint8_t trigger_types[XYPARSER_SAMPLES_PER_FRAME]; - std::uint8_t trigger_indices[XYPARSER_SAMPLES_PER_FRAME]; + std::uint8_t sample_trigger_types[XYPARSER_SAMPLES_PER_FRAME]; + std::uint8_t sample_trigger_indices[XYPARSER_SAMPLES_PER_FRAME]; }; +struct XYParserImpedanceSummary { + std::uint8_t channel_count; + std::uint32_t sample_rate; + std::uint32_t window_sample_count; + std::uint16_t impedance_values[XYPARSER_MAX_CHANNELS]; +}; + +struct XYParserWelchSummary { + std::uint8_t ok; + std::uint8_t channel_count; + std::uint32_t sample_rate; + std::uint32_t window_sample_count; + std::uint32_t frequency_count; + double frequencies[XYPARSER_WELCH_MAX_FREQUENCY_COUNT]; + double psd_values[XYPARSER_MAX_CHANNELS][XYPARSER_WELCH_MAX_FREQUENCY_COUNT]; + double band_values[XYPARSER_WELCH_BAND_COUNT][XYPARSER_MAX_CHANNELS]; +}; + +// 创建解析器实例。 +// @param channel_count 解析器支持的导联数。 +// @return 创建成功时返回解析器句柄，失败时返回 nullptr。 XYPARSER_API XYParserHandle XYParser_CreateParser(std::uint8_t channel_count); + +// 销毁解析器实例并释放相关资源。 +// @param handle 要销毁的解析器句柄。 +// @return 无返回值。 XYPARSER_API void XYParser_DestroyParser(XYParserHandle handle); + +// 设置 ADC 参考电压和增益参数。 +// @param handle 目标解析器句柄。 +// @param vref ADC 的参考电压。 +// @param gain ADC 的增益值。 +// @return 无返回值。 XYPARSER_API void XYParser_SetAdcParams(XYParserHandle handle, double vref, double gain); + +// 设置是否跳过校验和检查。 +// @param handle 目标解析器句柄。 +// @param bypass 非 0 表示跳过校验和检查，0 表示执行校验和检查。 +// @return 无返回值。 XYPARSER_API void XYParser_SetBypassChecksum(XYParserHandle handle, int bypass); + +// 设置解析器使用的采样率。 +// @param handle 目标解析器句柄。 +// @param sample_rate 采样率，单位为 Hz。 +// @return 无返回值。 +XYPARSER_API void XYParser_SetSampleRate(XYParserHandle handle, int sample_rate); + +// Frame parsing and algorithm result reading. +// 向解析器输入原始数据并读取解析得到的帧。 +// @param handle 目标解析器句柄。 +// @param data 输入的原始字节数据。 +// @param size 输入数据的字节数。 +// @param out_summaries 输出帧数组。 +// @param max_summaries 输出数组可写入的最大帧数，用于避免越界写入。 +// @return 本次实际输出的帧数；如果参数无效则返回 0。 XYPARSER_API int XYParser_Feed(XYParserHandle handle, const std::uint8_t* data, std::size_t size, XYParserFrameSummary* out_summaries, int max_summaries); + +// 设置是否启用阻抗检测。 +// @param handle 目标解析器句柄。 +// @param enabled 非 0 表示启用阻抗检测，0 表示禁用阻抗检测。 +// @return 无返回值。 +XYPARSER_API void XYParser_SetImpedanceDetection(XYParserHandle handle, int enabled); + +// 读取当前可用的阻抗结果。 +// @param handle 目标解析器句柄。 +// @param out_summaries 输出的阻抗结果数组。 +// @param max_summaries 输出数组可写入的最大结果数，用于避免越界写入。 +// @return 本次实际读取的结果数；如果参数无效则返回 0。 +XYPARSER_API int XYParser_ReadImpedance(XYParserHandle handle, + XYParserImpedanceSummary* out_summaries, + int max_summaries); + +// 设置是否启用基于算法数据的 Welch 检测。 +// @param handle 目标解析器句柄。 +// @param enabled 非 0 表示启用 Welch 检测，0 表示禁用 Welch 检测。 +// @return 无返回值。 +XYPARSER_API void XYParser_SetWelchDetection(XYParserHandle handle, int enabled); + +// 读取当前基于算法数据计算得到的 Welch 结果。 +// @param handle 目标解析器句柄。 +// @param out_summaries 输出的 Welch 结果数组。 +// @param max_summaries 输出数组可写入的最大结果数，用于避免越界写入。 +// @return 本次实际读取的结果数；如果参数无效则返回 0。 +XYPARSER_API int XYParser_ReadWelch(XYParserHandle handle, + XYParserWelchSummary* out_summaries, + int max_summaries); + +// Command serialization. +// 获取 8 导阻抗命令的字节长度。 +// @return 8 导阻抗命令的字节长度。 +XYPARSER_API std::size_t XYParser_Get8ChImpedanceCommandSize(); + +// 生成 8 导阻抗开关命令。 +// @param open 非 0 表示打开阻抗检测，0 表示关闭阻抗检测。 +// @param out_command 输出的命令缓冲区。 +// @param command_size 输出缓冲区大小，单位为字节。 +// @return 生成成功时返回写入的字节数，失败时返回 0。 +XYPARSER_API int XYParser_Serialize8ChImpedanceCommand(int open, + std::uint8_t* out_command, + std::size_t command_size); + +// 获取 64 导阻抗命令的字节长度。 +// @return 64 导阻抗命令的字节长度。 +XYPARSER_API std::size_t XYParser_Get64ImpedanceCommandSize(void); + +// 生成 64 导阻抗开关命令。 +// @param open 非 0 表示打开阻抗检测，0 表示关闭阻抗检测。 +// @param out_buffer 输出的命令缓冲区。 +// @param buffer_size 输出缓冲区大小，单位为字节。 +// @return 生成成功时返回写入的字节数，失败时返回 0。 +XYPARSER_API int XYParser_Serialize64ImpedanceCommand(int open, + std::uint8_t* out_buffer, + std::size_t buffer_size); + +// 获取 64 导增益和采样率命令的字节长度。 +// @return 64 导增益和采样率命令的字节长度。 +XYPARSER_API std::size_t XYParser_Get64GainSampleRateCommandSize(void); + +// 生成 64 导增益和采样率设置命令。 +// @param gain 要设置的增益值。 +// @param sample_rate 要设置的采样率。 +// @param out_buffer 输出的命令缓冲区。 +// @param buffer_size 输出缓冲区大小，单位为字节。 +// @return 生成成功时返回写入的字节数，失败时返回 0。 +XYPARSER_API int XYParser_Serialize64GainSampleRateCommand(std::uint8_t gain, + std::uint16_t sample_rate, + std::uint8_t* out_buffer, + std::size_t buffer_size); + +// Frame conversion and algorithm data output. +// 将 8 导帧批量转换为 64 导帧，未映射的导联补 0。 +// @param input_summaries 输入的 8 导帧数组。 +// @param input_count 输入数组中的帧数。 +// @param output_summaries 输出的 64 导帧数组。 +// @param max_summaries 输出数组可写入的最大帧数，用于避免越界写入。 +// @return 实际成功转换的帧数；如果参数无效则返回 0，如果中途遇到非 8 导帧则返回已完成转换的数量。 +XYPARSER_API int XYParser_Convert8ChFramesTo64Ch(const XYParserFrameSummary* input_summaries, + int input_count, + XYParserFrameSummary* output_summaries, + int max_summaries); + +// 获取算法数据数组所需的元素数量。 +// @return 算法数据数组所需的元素数量。 +XYPARSER_API std::size_t XYParser_GetAlgorithmDataValueCount(); + +// 将单帧数据转换为算法接口使用的连续数组。 +// @param input_summary 输入的单帧数据。 +// @param output_data 输出的算法数据数组。 +// @return 转换成功时返回非 0，失败时返回 0。 +XYPARSER_API int XYParser_ConvertSampleFramesToAlgorithmData(const XYParserFrameSummary* input_summary, + double* output_data); + +// 向算法数据缓存输入字节流，并按每 5 个采样组装为帧，同时驱动 Welch 计算。 +// @param handle 目标解析器句柄。 +// @param data 输入的算法数据字节流。 +// @param size 输入数据的字节数。 +// @param out_summaries 输出的帧数组。 +// @param max_summaries 输出数组可写入的最大帧数，用于避免越界写入。 +// @return 实际输出的帧数；如果参数无效则返回 0。 +XYPARSER_API int XYParser_FeedAlgorithmData(XYParserHandle handle, + const std::uint8_t* data, + std::size_t size, + XYParserFrameSummary* out_summaries, + int max_summaries); + +// 清空算法数据缓存。 +// @param handle 目标解析器句柄。 +// @return 无返回值。 +XYPARSER_API void XYParser_ResetAlgorithmDataCache(XYParserHandle handle); + +// 将缓存中不足 5 个采样的尾数据补齐并输出为 1 帧。 +// @param handle 目标解析器句柄。 +// @param out_summary 输出的帧。 +// @return 成功输出 1 帧时返回 1；没有可输出的数据或参数无效时返回 0。 +XYPARSER_API int XYParser_FlushAlgorithmData(XYParserHandle handle, + XYParserFrameSummary* out_summary); + +// Lead and Welch metadata. +// 获取指定导联数对应的导联映射数量。 +// @param channel_count 导联数。 +// @return 导联映射数量；如果导联数不支持则返回 0。 +XYPARSER_API int XYParser_GetLeadMapCount(std::uint8_t channel_count); + +// 获取指定导联数对应的导联映射表。 +// @param channel_count 导联数。 +// @param out_leads 输出的导联映射数组。 +// @param max_leads 输出数组可写入的最大导联数，用于避免越界写入。 +// @return 实际写入的导联数；如果参数无效则返回 0。 +XYPARSER_API int XYParser_GetLeadMap(std::uint8_t channel_count, + XYParserLeadChannelNumber* out_leads, + int max_leads); + +// 获取导联编号对应的名称字符串。 +// @param lead 导联编号。 +// @return 导联名称字符串；如果导联编号无效则返回 nullptr。 +XYPARSER_API const char* XYParser_GetLeadName(XYParserLeadChannelNumber lead); + +// 获取 Welch 频段索引对应的名称字符串。 +// @param band_index Welch 频段索引。 +// @return 频段名称字符串；如果索引无效则返回 nullptr。 +XYPARSER_API const char* XYParser_GetWelchBandName(int band_index); + +// 获取 Welch 频段索引对应的频率范围。 +// @param band_index Welch 频段索引。 +// @param out_low_hz 输出的频段下限频率，单位为 Hz。 +// @param out_high_hz 输出的频段上限频率，单位为 Hz。 +// @return 获取成功时返回非 0，失败时返回 0。 +XYPARSER_API int XYParser_GetWelchBandRange(int band_index, + double* out_low_hz, + double* out_high_hz); + +// 获取解析器最近一次错误的描述信息。 +// @param handle 目标解析器句柄。 +// @return 错误描述字符串；如果当前没有错误信息则返回 nullptr。 XYPARSER_API const char* XYParser_GetLastError(XYParserHandle handle); } diff --git a/XYParser/XYParserTests/Tests.cpp b/XYParser/XYParserTests/Tests.cpp index 7335666..6bb8d10 100644 --- a/XYParser/XYParserTests/Tests.cpp +++ b/XYParser/XYParserTests/Tests.cpp @@ -4,9 +4,11 @@ #include #include "../XYParserApi.h" +#include #include #include #include +#include #include #include @@ -106,6 +108,70 @@ std::vector BuildMinimalFrame(std::uint8_t channel_count) return BuildMinimalFrame(channel_count, 1U); } +void WriteSigned24(std::vector& frame, std::size_t& offset, int raw_value) +{ + constexpr int kMinSigned24 = -8388608; + constexpr int kMaxSigned24 = 8388607; + raw_value = std::max(kMinSigned24, std::min(kMaxSigned24, raw_value)); + std::uint32_t encoded = 0; + if (raw_value < 0) { + encoded = static_cast((1 << 24) + raw_value); + } else { + encoded = static_cast(raw_value); + } + + frame[offset++] = static_cast((encoded >> 16) & 0xFF); + frame[offset++] = static_cast((encoded >> 8) & 0xFF); + frame[offset++] = static_cast(encoded & 0xFF); +} + +std::vector BuildFrameWithRawSamples( + std::uint8_t channel_count, + std::uint32_t frame_index, + const std::array, XYPARSER_SAMPLES_PER_FRAME>& raw_samples) +{ + constexpr std::uint8_t kHeader = 0xAA; + constexpr std::uint8_t kTail = 0x55; + constexpr std::size_t kTagLen = 25; + + const std::size_t sample_bytes = static_cast(channel_count) * 3 + 2; + const std::uint16_t payload_length = static_cast(sample_bytes * XYPARSER_SAMPLES_PER_FRAME); + const std::size_t frame_size = 1 + kTagLen + payload_length + 2; + + std::vector frame(frame_size, 0); + std::size_t offset = 0; + frame[offset++] = kHeader; + frame[offset++] = static_cast(frame_index & 0xFF); + frame[offset++] = static_cast((frame_index >> 8) & 0xFF); + frame[offset++] = static_cast((frame_index >> 16) & 0xFF); + frame[offset++] = static_cast((frame_index >> 24) & 0xFF); + frame[offset++] = static_cast((payload_length >> 8) & 0xFF); + frame[offset++] = static_cast(payload_length & 0xFF); + frame[offset++] = 95; + frame[offset++] = channel_count; + offset += 2 + 2 + 2 + 2 + 2 + 6; + + for (std::size_t sample = 0; sample < XYPARSER_SAMPLES_PER_FRAME; ++sample) { + for (std::size_t channel = 0; channel < channel_count; ++channel) { + WriteSigned24(frame, offset, raw_samples[sample][channel]); + } + frame[offset++] = 0x00; + frame[offset++] = 0x00; + } + + frame[offset++] = 0x00; + frame[offset++] = kTail; + frame[offset++] = kTail; + return frame; +} + +int BuildSineRawValue(int sample_index, int sample_rate, double frequency_hz, double amplitude) +{ + const double angle = 2.0 * 3.14159265358979323846 * frequency_hz * + static_cast(sample_index) / static_cast(sample_rate); + return static_cast(std::lround(std::sin(angle) * amplitude)); +} + } // namespace /// 测试：创建解析器时拒绝不支持的通道数 @@ -122,6 +188,507 @@ TEST(XYParserApiTests, GetLastErrorReturnsMessageForNullParser) EXPECT_EQ(std::string(XYParser_GetLastError(nullptr)), std::string("invalid parser handle")); } +TEST(XYParserApiTests, Serialize8ChImpedanceOpenCommandMatchesWirelessEegPacket) +{ + constexpr std::size_t kCommandSize = 7; + std::array command{}; + const int command_size = XYParser_Serialize8ChImpedanceCommand(1, command.data(), command.size()); + + ASSERT_EQ(command_size, static_cast(kCommandSize)); + const std::array expected = { + 0xAA, 0x00, 0x01, 0xA1, 0xA1, 0x55, 0x55}; + EXPECT_EQ(command, expected); +} + +TEST(XYParserApiTests, Serialize8ChImpedanceCloseCommandMatchesWirelessEegPacket) +{ + constexpr std::size_t kCommandSize = 7; + std::array command{}; + const int command_size = XYParser_Serialize8ChImpedanceCommand(0, command.data(), command.size()); + + ASSERT_EQ(command_size, static_cast(kCommandSize)); + const std::array expected = { + 0xAA, 0x00, 0x01, 0xA0, 0xA0, 0x55, 0x55}; + EXPECT_EQ(command, expected); +} + +TEST(XYParserApiTests, Serialize8ChImpedanceCommandRejectsSmallBuffer) +{ + std::array command{}; + EXPECT_EQ(XYParser_Serialize8ChImpedanceCommand(1, command.data(), command.size()), 0); +} + +TEST(XYParserApiTests, Get8ChImpedanceCommandSizeMatchesSerializedLength) +{ + EXPECT_EQ(XYParser_Get8ChImpedanceCommandSize(), static_cast(7)); +} + +TEST(XYParserApiTests, GetAlgorithmDataValueCountMatchesFrameLayout) +{ + EXPECT_EQ(XYParser_GetAlgorithmDataValueCount(), + static_cast(XYPARSER_FRAME_ALGORITHM_VALUE_COUNT)); +} + +TEST(XYParserApiTests, GetLeadMapCountReturnsExpectedCounts) +{ + EXPECT_EQ(XYParser_GetLeadMapCount(8), 8); + EXPECT_EQ(XYParser_GetLeadMapCount(64), 64); + EXPECT_EQ(XYParser_GetLeadMapCount(7), 0); +} + +TEST(XYParserApiTests, GetLeadMapReturnsExpected8ChannelSubset) +{ + std::array leads{}; + const int count = XYParser_GetLeadMap(8, leads.data(), static_cast(leads.size())); + + ASSERT_EQ(count, 8); + const std::array expected = { + LeadChannel_PO5, LeadChannel_POZ, LeadChannel_PO6, LeadChannel_PO7, + LeadChannel_O1, LeadChannel_OZ, LeadChannel_O2, LeadChannel_PO8}; + EXPECT_EQ(leads, expected); +} + +TEST(XYParserApiTests, GetLeadMapReturnsExpected64ChannelOrder) +{ + std::array leads{}; + const int count = XYParser_GetLeadMap(64, leads.data(), static_cast(leads.size())); + + ASSERT_EQ(count, 64); + EXPECT_EQ(leads.front(), LeadChannel_FP1); + EXPECT_EQ(leads[1], LeadChannel_FP2); + EXPECT_EQ(leads[13], LeadChannel_PO5); + EXPECT_EQ(leads[55], LeadChannel_PO7); + EXPECT_EQ(leads.back(), LeadChannel_F1); +} + +TEST(XYParserApiTests, GetLeadMapRejectsUnsupportedChannelCountOrSmallBuffer) +{ + std::array small_buffer{}; + EXPECT_EQ(XYParser_GetLeadMap(8, small_buffer.data(), static_cast(small_buffer.size())), 0); + EXPECT_EQ(XYParser_GetLeadMap(7, nullptr, 0), 0); +} + +TEST(XYParserApiTests, Convert8ChFramesTo64ChMapsKnownLeadsAndPadsOthersWithZero) +{ + std::array input{}; + input[0].frame_index = 7U; + input[0].channel_count = 8U; + input[0].battery = 88U; + input[0].sample_count = 5U; + input[0].sample_trigger_types[0] = 0xB2; + input[0].sample_trigger_indices[0] = 3U; + input[0].channel_values_uv[0][0] = 11.0; + input[0].channel_values_uv[0][1] = 12.0; + input[0].channel_values_uv[0][2] = 13.0; + input[0].channel_values_uv[0][3] = 14.0; + input[0].channel_values_uv[0][4] = 15.0; + input[0].channel_values_uv[0][5] = 16.0; + input[0].channel_values_uv[0][6] = 17.0; + input[0].channel_values_uv[0][7] = 18.0; + + input[1].frame_index = 8U; + input[1].channel_count = 8U; + input[1].battery = 77U; + input[1].sample_count = 5U; + input[1].channel_values_uv[1][0] = 21.0; + + std::array output{}; + ASSERT_EQ(XYParser_Convert8ChFramesTo64Ch(input.data(), static_cast(input.size()), output.data(), static_cast(output.size())), 2); + EXPECT_EQ(output[0].frame_index, 7U); + EXPECT_EQ(output[0].channel_count, 64U); + EXPECT_EQ(output[0].battery, 88U); + EXPECT_EQ(output[0].sample_count, 5U); + EXPECT_EQ(output[0].sample_trigger_types[0], 0xB2); + EXPECT_EQ(output[0].sample_trigger_indices[0], 3U); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_PO5], 11.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_POZ], 12.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_PO6], 13.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_PO7], 14.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_O1], 15.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_OZ], 16.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_O2], 17.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_PO8], 18.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_FP1], 0.0); + EXPECT_DOUBLE_EQ(output[0].channel_values_uv[0][LeadChannel_CZ], 0.0); + EXPECT_EQ(output[1].frame_index, 8U); + EXPECT_EQ(output[1].channel_count, 64U); + EXPECT_DOUBLE_EQ(output[1].channel_values_uv[1][LeadChannel_PO5], 21.0); +} + +TEST(XYParserApiTests, Convert8ChFramesTo64ChRejectsInvalidArgumentsAndStopsAtNon8ChannelInput) +{ + std::array input{}; + input[0].channel_count = 8U; + input[1].channel_count = 64U; + std::array output{}; + + EXPECT_EQ(XYParser_Convert8ChFramesTo64Ch(nullptr, 1, output.data(), static_cast(output.size())), 0); + EXPECT_EQ(XYParser_Convert8ChFramesTo64Ch(input.data(), 1, nullptr, static_cast(output.size())), 0); + EXPECT_EQ(XYParser_Convert8ChFramesTo64Ch(input.data(), 0, output.data(), static_cast(output.size())), 0); + EXPECT_EQ(XYParser_Convert8ChFramesTo64Ch(input.data(), static_cast(input.size()), output.data(), static_cast(output.size())), 1); + EXPECT_EQ(output[0].channel_count, 64U); +} + +TEST(XYParserApiTests, ConvertSampleFramesToAlgorithmDataAppendsTriggerColumnsPerSample) +{ + constexpr std::size_t kFrameValueCount = XYPARSER_FRAME_ALGORITHM_VALUE_COUNT; + XYParserFrameSummary input{}; + input.channel_count = 64U; + input.sample_count = 5U; + input.channel_values_uv[0][0] = 101.0; + input.channel_values_uv[0][63] = 164.0; + input.sample_trigger_types[0] = 0xA5; + input.sample_trigger_indices[0] = 9U; + input.channel_values_uv[1][10] = 210.0; + input.sample_trigger_types[1] = 0x7F; + input.sample_trigger_indices[1] = 5U; + input.channel_values_uv[4][3] = 403.0; + input.sample_trigger_types[4] = 0xB2; + input.sample_trigger_indices[4] = 11U; + + std::array output{}; + ASSERT_EQ(XYParser_ConvertSampleFramesToAlgorithmData(&input, output.data()), 1); + + const std::size_t sample0 = 0; + const std::size_t sample1 = XYPARSER_FRAME_DATA_COLUMN_COUNT; + const std::size_t sample4 = 4 * XYPARSER_FRAME_DATA_COLUMN_COUNT; + + EXPECT_DOUBLE_EQ(output[sample0 + 0], 101.0); + EXPECT_DOUBLE_EQ(output[sample0 + 63], 164.0); + EXPECT_DOUBLE_EQ(output[sample0 + XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX], 165.0); + EXPECT_DOUBLE_EQ(output[sample0 + XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX], 9.0); + EXPECT_DOUBLE_EQ(output[sample4 + 3], 403.0); + EXPECT_DOUBLE_EQ(output[sample4 + XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX], 178.0); + EXPECT_DOUBLE_EQ(output[sample4 + XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX], 11.0); + EXPECT_DOUBLE_EQ(output[sample1 + 10], 210.0); + EXPECT_DOUBLE_EQ(output[sample1 + XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX], 127.0); + EXPECT_DOUBLE_EQ(output[sample1 + XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX], 5.0); +} + +TEST(XYParserApiTests, ConvertSampleFramesToAlgorithmDataRejectsInvalidArgumentsAndNon64ChannelInput) +{ + constexpr std::size_t kFrameValueCount = XYPARSER_FRAME_ALGORITHM_VALUE_COUNT; + XYParserFrameSummary input{}; + input.channel_count = 8U; + std::array output{}; + + EXPECT_EQ(XYParser_ConvertSampleFramesToAlgorithmData(nullptr, output.data()), 0); + EXPECT_EQ(XYParser_ConvertSampleFramesToAlgorithmData(&input, nullptr), 0); + EXPECT_EQ(XYParser_ConvertSampleFramesToAlgorithmData(&input, output.data()), 0); + EXPECT_DOUBLE_EQ(output[XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX], 0.0); +} + +TEST(XYParserApiTests, FeedAlgorithmDataCachesSamplesBuildsFramesAndFlushesTailSamples) +{ + ParserGuard parser(XYParser_CreateParser(64)); + ASSERT_NE(parser.get(), nullptr); + + constexpr int kTotalSamples = 7; + constexpr int kColumnCount = XYPARSER_FRAME_DATA_COLUMN_COUNT; + std::array(kTotalSamples) * kColumnCount> input_data{}; + for (int sample_index = 0; sample_index < kTotalSamples; ++sample_index) { + const std::size_t row_offset = static_cast(sample_index) * kColumnCount; + input_data[row_offset + 0] = 1000.0 + sample_index; + input_data[row_offset + 10] = 2000.0 + sample_index; + input_data[row_offset + 63] = 3000.0 + sample_index; + input_data[row_offset + XYPARSER_FRAME_DATA_TRIGGER_TYPE_INDEX] = 10.0 + sample_index; + input_data[row_offset + XYPARSER_FRAME_DATA_TRIGGER_INDEX_INDEX] = 20.0 + sample_index; + } + const std::uint8_t* input_bytes = reinterpret_cast(input_data.data()); + const std::size_t first_chunk_size = 3 * static_cast(kColumnCount) * sizeof(double) + sizeof(double); + const std::size_t second_chunk_size = sizeof(input_data) - first_chunk_size; + + std::array summaries{}; + EXPECT_EQ(XYParser_FeedAlgorithmData( + parser.get(), + input_bytes, + first_chunk_size, + summaries.data(), + static_cast(summaries.size())), + 0); + + ASSERT_EQ(XYParser_FeedAlgorithmData( + parser.get(), + input_bytes + first_chunk_size, + second_chunk_size, + summaries.data(), + static_cast(summaries.size())), + 1); + EXPECT_EQ(summaries[0].frame_index, 1U); + EXPECT_EQ(summaries[0].channel_count, 64U); + EXPECT_EQ(summaries[0].sample_count, 5U); + EXPECT_DOUBLE_EQ(summaries[0].channel_values_uv[0][0], 1000.0); + EXPECT_DOUBLE_EQ(summaries[0].channel_values_uv[2][10], 2002.0); + EXPECT_DOUBLE_EQ(summaries[0].channel_values_uv[4][63], 3004.0); + EXPECT_EQ(summaries[0].sample_trigger_types[0], 10U); + EXPECT_EQ(summaries[0].sample_trigger_indices[4], 24U); + + XYParserFrameSummary tail_summary{}; + ASSERT_EQ(XYParser_FlushAlgorithmData(parser.get(), &tail_summary), 1); + EXPECT_EQ(tail_summary.frame_index, 2U); + EXPECT_EQ(tail_summary.channel_count, 64U); + EXPECT_EQ(tail_summary.sample_count, 2U); + EXPECT_DOUBLE_EQ(tail_summary.channel_values_uv[0][0], 1005.0); + EXPECT_DOUBLE_EQ(tail_summary.channel_values_uv[1][63], 3006.0); + EXPECT_EQ(tail_summary.sample_trigger_types[0], 15U); + EXPECT_EQ(tail_summary.sample_trigger_indices[1], 26U); + EXPECT_DOUBLE_EQ(tail_summary.channel_values_uv[2][0], 0.0); + EXPECT_EQ(tail_summary.sample_trigger_types[2], 0U); + + XYParser_ResetAlgorithmDataCache(parser.get()); + EXPECT_EQ(XYParser_FlushAlgorithmData(parser.get(), &tail_summary), 0); +} + +TEST(XYParserApiTests, GetLeadNameReturnsExpectedNames) +{ + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_FP1)), "FP1"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_PO5)), "PO5"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_OZ)), "OZ"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_GND)), "GND"); +} + +TEST(XYParserApiTests, GetLeadNameCovers8ChannelSubsetNames) +{ + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_POZ)), "POZ"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_PO6)), "PO6"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_PO7)), "PO7"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_PO8)), "PO8"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_O1)), "O1"); + EXPECT_EQ(std::string(XYParser_GetLeadName(LeadChannel_O2)), "O2"); +} + +TEST(XYParserApiTests, GetLeadNameReturnsEmptyStringForInvalidLead) +{ + EXPECT_EQ(std::string(XYParser_GetLeadName(static_cast(255))), ""); +} + +TEST(XYParserApiTests, GetWelchBandNameReturnsExpectedNames) +{ + EXPECT_EQ(std::string(XYParser_GetWelchBandName(0)), "delta"); + EXPECT_EQ(std::string(XYParser_GetWelchBandName(1)), "theta"); + EXPECT_EQ(std::string(XYParser_GetWelchBandName(2)), "alpha"); + EXPECT_EQ(std::string(XYParser_GetWelchBandName(3)), "beta"); + EXPECT_EQ(std::string(XYParser_GetWelchBandName(4)), "gamma"); +} + +TEST(XYParserApiTests, GetWelchBandNameReturnsEmptyStringForInvalidIndex) +{ + EXPECT_EQ(std::string(XYParser_GetWelchBandName(-1)), ""); + EXPECT_EQ(std::string(XYParser_GetWelchBandName(XYPARSER_WELCH_BAND_COUNT)), ""); +} + +TEST(XYParserApiTests, GetWelchBandRangeReturnsExpectedRanges) +{ + double low_hz = 0.0; + double high_hz = 0.0; + + ASSERT_EQ(XYParser_GetWelchBandRange(0, &low_hz, &high_hz), 1); + EXPECT_DOUBLE_EQ(low_hz, 0.5); + EXPECT_DOUBLE_EQ(high_hz, 4.0); + + ASSERT_EQ(XYParser_GetWelchBandRange(4, &low_hz, &high_hz), 1); + EXPECT_DOUBLE_EQ(low_hz, 30.0); + EXPECT_DOUBLE_EQ(high_hz, 50.0); +} + +TEST(XYParserApiTests, GetWelchBandRangeRejectsInvalidArguments) +{ + double low_hz = 0.0; + double high_hz = 0.0; + + EXPECT_EQ(XYParser_GetWelchBandRange(-1, &low_hz, &high_hz), 0); + EXPECT_EQ(XYParser_GetWelchBandRange(XYPARSER_WELCH_BAND_COUNT, &low_hz, &high_hz), 0); + EXPECT_EQ(XYParser_GetWelchBandRange(0, nullptr, &high_hz), 0); + EXPECT_EQ(XYParser_GetWelchBandRange(0, &low_hz, nullptr), 0); +} + +TEST(XYParserApiTests, SetImpedanceDetectionSwitchesParserGainBetween24And6) +{ + ParserGuard parser(XYParser_CreateParser(8)); + ASSERT_NE(parser.get(), nullptr); + + XYParser_SetAdcParams(parser.get(), 4.5, 6.0); + XYParser_SetBypassChecksum(parser.get(), 1); + + const std::vector frame1 = BuildMinimalFrame(8, 1U); + const std::vector frame2 = BuildMinimalFrame(8, 2U); + const std::vector frame3 = BuildMinimalFrame(8, 3U); + std::array summaries{}; + + ASSERT_EQ(XYParser_Feed(parser.get(), frame1.data(), frame1.size(), summaries.data(), static_cast(summaries.size())), 1); + const double gain6_value_before = summaries[0].channel_values_uv[0][0]; + + XYParser_SetImpedanceDetection(parser.get(), 1); + ASSERT_EQ(XYParser_Feed(parser.get(), frame2.data(), frame2.size(), summaries.data(), static_cast(summaries.size())), 1); + const double gain24_value = summaries[0].channel_values_uv[0][0]; + + XYParser_SetImpedanceDetection(parser.get(), 0); + ASSERT_EQ(XYParser_Feed(parser.get(), frame3.data(), frame3.size(), summaries.data(), static_cast(summaries.size())), 1); + const double gain6_value_after = summaries[0].channel_values_uv[0][0]; + + EXPECT_NEAR(gain24_value * 4.0, gain6_value_before, 1e-9); + EXPECT_NEAR(gain6_value_after, gain6_value_before, 1e-9); +} + +TEST(XYParserApiTests, ImpedanceReturnsOneResultAfterOneSecondFor8Channels) +{ + ParserGuard parser(XYParser_CreateParser(8)); + ASSERT_NE(parser.get(), nullptr); + + XYParser_SetBypassChecksum(parser.get(), 1); + XYParser_SetSampleRate(parser.get(), 10); + XYParser_SetImpedanceDetection(parser.get(), 1); + + std::array, XYPARSER_SAMPLES_PER_FRAME> raw_samples1{}; + std::array, XYPARSER_SAMPLES_PER_FRAME> raw_samples2{}; + for (int sample = 0; sample < 10; ++sample) { + const int raw_value = BuildSineRawValue(sample, 10, 2.0, 1000000.0); + if (sample < static_cast(XYPARSER_SAMPLES_PER_FRAME)) { + raw_samples1[static_cast(sample)][0] = raw_value; + } else { + raw_samples2[static_cast(sample - XYPARSER_SAMPLES_PER_FRAME)][0] = raw_value; + } + } + + const std::vector frame1 = BuildFrameWithRawSamples(8, 1U, raw_samples1); + const std::vector frame2 = BuildFrameWithRawSamples(8, 2U, raw_samples2); + std::array summaries{}; + std::array impedance{}; + + EXPECT_EQ(XYParser_Feed(parser.get(), frame1.data(), frame1.size(), summaries.data(), static_cast(summaries.size())), 1); + EXPECT_EQ(XYParser_ReadImpedance(parser.get(), impedance.data(), static_cast(impedance.size())), 0); + + EXPECT_EQ(XYParser_Feed(parser.get(), frame2.data(), frame2.size(), summaries.data(), static_cast(summaries.size())), 1); + ASSERT_EQ(XYParser_ReadImpedance(parser.get(), impedance.data(), static_cast(impedance.size())), 1); + EXPECT_EQ(impedance[0].channel_count, 8); + EXPECT_EQ(impedance[0].sample_rate, 10U); + EXPECT_EQ(impedance[0].window_sample_count, 10U); + EXPECT_GT(impedance[0].impedance_values[LeadChannel_PO5], 0); + EXPECT_EQ(impedance[0].impedance_values[LeadChannel_FP1], 0); +} + +TEST(XYParserApiTests, ImpedanceUsesUnifiedLeadIndexesFor64Channels) +{ + ParserGuard parser(XYParser_CreateParser(64)); + ASSERT_NE(parser.get(), nullptr); + + XYParser_SetBypassChecksum(parser.get(), 1); + XYParser_SetSampleRate(parser.get(), 10); + XYParser_SetImpedanceDetection(parser.get(), 1); + + std::array, XYPARSER_SAMPLES_PER_FRAME> raw_samples1{}; + std::array, XYPARSER_SAMPLES_PER_FRAME> raw_samples2{}; + for (int sample = 0; sample < 10; ++sample) { + const int raw_value = BuildSineRawValue(sample, 10, 3.0, 1000000.0); + if (sample < static_cast(XYPARSER_SAMPLES_PER_FRAME)) { + raw_samples1[static_cast(sample)][0] = raw_value; + } else { + raw_samples2[static_cast(sample - XYPARSER_SAMPLES_PER_FRAME)][0] = raw_value; + } + } + + const std::vector frame1 = BuildFrameWithRawSamples(64, 1U, raw_samples1); + const std::vector frame2 = BuildFrameWithRawSamples(64, 2U, raw_samples2); + std::array summaries{}; + std::array impedance{}; + + EXPECT_EQ(XYParser_Feed(parser.get(), frame1.data(), frame1.size(), summaries.data(), static_cast(summaries.size())), 1); + EXPECT_EQ(XYParser_Feed(parser.get(), frame2.data(), frame2.size(), summaries.data(), static_cast(summaries.size())), 1); + ASSERT_EQ(XYParser_ReadImpedance(parser.get(), impedance.data(), static_cast(impedance.size())), 1); + EXPECT_EQ(impedance[0].channel_count, 64); + EXPECT_GT(impedance[0].impedance_values[LeadChannel_FP1], 0); + EXPECT_EQ(impedance[0].impedance_values[LeadChannel_FP2], 0); +} + +TEST(XYParserApiTests, WelchReturnsOneResultAfterOneSecondFromAlgorithmData) +{ + ParserGuard parser(XYParser_CreateParser(64)); + ASSERT_NE(parser.get(), nullptr); + + XYParser_SetSampleRate(parser.get(), 10); + XYParser_SetWelchDetection(parser.get(), 1); + + std::vector algorithm_data(static_cast(10 * XYPARSER_FRAME_DATA_COLUMN_COUNT), 0.0); + for (int sample = 0; sample < 10; ++sample) { + const std::size_t sample_offset = static_cast(sample) * XYPARSER_FRAME_DATA_COLUMN_COUNT; + algorithm_data[sample_offset] = static_cast(BuildSineRawValue(sample, 10, 2.0, 1000000.0)); + } + + std::array welch{}; + + EXPECT_EQ(XYParser_FeedAlgorithmData( + parser.get(), + reinterpret_cast(algorithm_data.data()), + algorithm_data.size() * sizeof(double), + nullptr, + 0), + 0); + ASSERT_EQ(XYParser_ReadWelch(parser.get(), welch.data(), static_cast(welch.size())), 1); + EXPECT_EQ(welch[0].ok, 1); + EXPECT_EQ(welch[0].channel_count, 64); + EXPECT_EQ(welch[0].sample_rate, 10U); + EXPECT_EQ(welch[0].window_sample_count, 10U); + EXPECT_GT(welch[0].frequency_count, 0U); + EXPECT_DOUBLE_EQ(welch[0].frequencies[0], 1.0); + EXPECT_GT(welch[0].psd_values[LeadChannel_FP1][0], 0.0); + EXPECT_EQ(welch[0].psd_values[LeadChannel_FP2][0], 0.0); + EXPECT_GT(welch[0].band_values[0][LeadChannel_FP1], 0.0); +} + +TEST(XYParserApiTests, WelchFrameFeedDoesNotProduceResults) +{ + ParserGuard parser(XYParser_CreateParser(64)); + ASSERT_NE(parser.get(), nullptr); + + XYParser_SetBypassChecksum(parser.get(), 1); + XYParser_SetSampleRate(parser.get(), 10); + XYParser_SetWelchDetection(parser.get(), 1); + + std::array, XYPARSER_SAMPLES_PER_FRAME> raw_samples1{}; + std::array, XYPARSER_SAMPLES_PER_FRAME> raw_samples2{}; + for (int sample = 0; sample < 10; ++sample) { + const int raw_value = BuildSineRawValue(sample, 10, 2.0, 1000000.0); + if (sample < static_cast(XYPARSER_SAMPLES_PER_FRAME)) { + raw_samples1[static_cast(sample)][0] = raw_value; + } else { + raw_samples2[static_cast(sample - XYPARSER_SAMPLES_PER_FRAME)][0] = raw_value; + } + } + + const std::vector frame1 = BuildFrameWithRawSamples(64, 1U, raw_samples1); + const std::vector frame2 = BuildFrameWithRawSamples(64, 2U, raw_samples2); + std::array summaries{}; + std::array welch{}; + + EXPECT_EQ(XYParser_Feed(parser.get(), frame1.data(), frame1.size(), summaries.data(), static_cast(summaries.size())), 1); + EXPECT_EQ(XYParser_Feed(parser.get(), frame2.data(), frame2.size(), summaries.data(), static_cast(summaries.size())), 1); + EXPECT_EQ(XYParser_ReadWelch(parser.get(), welch.data(), static_cast(welch.size())), 0); +} + +TEST(XYParserApiTests, WelchDisabledDoesNotProduceResultsFromAlgorithmData) +{ + ParserGuard parser(XYParser_CreateParser(64)); + ASSERT_NE(parser.get(), nullptr); + + XYParser_SetSampleRate(parser.get(), 10); + + std::vector algorithm_data(static_cast(10 * XYPARSER_FRAME_DATA_COLUMN_COUNT), 0.0); + for (int sample = 0; sample < 10; ++sample) { + const std::size_t sample_offset = static_cast(sample) * XYPARSER_FRAME_DATA_COLUMN_COUNT; + algorithm_data[sample_offset] = static_cast(BuildSineRawValue(sample, 10, 2.0, 1000000.0)); + } + + std::array welch{}; + EXPECT_EQ(XYParser_FeedAlgorithmData( + parser.get(), + reinterpret_cast(algorithm_data.data()), + algorithm_data.size() * sizeof(double), + nullptr, + 0), + 0); + EXPECT_EQ(XYParser_ReadWelch(parser.get(), welch.data(), static_cast(welch.size())), 0); +} + /// 测试：Feed 函数能正确解析完整的 8 通道帧 TEST(XYParserApiTests, FeedParsesAComplete8ChannelFrame) { @@ -151,8 +718,8 @@ TEST(XYParserApiTests, FeedParsesAComplete8ChannelFrame) EXPECT_EQ(summaries[0].battery, 95U); // 电池电量应为 95 EXPECT_EQ(summaries[0].sample_count, 5U); // 采样数应为 5 EXPECT_GT(summaries[0].channel_values_uv[0][0], 0.0); // 通道值应大于 0 - EXPECT_EQ(summaries[0].trigger_types[0], 0U); // 触发类型应为 0 - EXPECT_EQ(summaries[0].trigger_indices[0], 0U); // 触发索引应为 0 + EXPECT_EQ(summaries[0].sample_trigger_types[0], 0U); // 触发类型应为 0 + EXPECT_EQ(summaries[0].sample_trigger_indices[0], 0U); // 触发索引应为 0 } /// 测试：Feed 函数能缓冲部分数据直到完整帧可用 @@ -268,6 +835,56 @@ TEST(XYParserApiTests, SetBypassChecksumOnNullHandle) EXPECT_NO_THROW(XYParser_SetBypassChecksum(nullptr, 1)); } +// ============================================================================ +// 64 导控制协议序列化测试 +// ============================================================================ + +/// 测试：64 导开启阻抗命令序列化与 WirelessEEG 协议保持一致 +TEST(XYParserApiTests, Serialize64ImpedanceOpenCommandMatchesWirelessEegProtocol) +{ + std::array buffer{}; + const int size = XYParser_Serialize64ImpedanceCommand(1, buffer.data(), buffer.size()); + + ASSERT_EQ(size, static_cast(XYParser_Get64ImpedanceCommandSize())); + + const std::vector expected = { + 0xAA, 0x01, 0x00, 0x07, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x09, 0x55, 0x55 + }; + EXPECT_TRUE(std::equal(expected.begin(), expected.end(), buffer.begin())); +} + +/// 测试：64 导增益和采样率命令序列化与 WirelessEEG 协议保持一致 +TEST(XYParserApiTests, Serialize64GainAndSampleRateCommandMatchesWirelessEegProtocol) +{ + std::array buffer{}; + const int size = XYParser_Serialize64GainSampleRateCommand(24, 1000, buffer.data(), buffer.size()); + + ASSERT_EQ(size, static_cast(XYParser_Get64GainSampleRateCommandSize())); + + const std::vector expected = { + 0xAA, 0x02, 0x00, 0x08, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x18, 0x02, 0x24, 0x55, 0x55 + }; + EXPECT_TRUE(std::equal(expected.begin(), expected.end(), buffer.begin())); +} + +/// 测试：64 导增益和采样率命令拒绝非法采样率 +TEST(XYParserApiTests, Serialize64GainAndSampleRateCommandRejectsUnsupportedSampleRate) +{ + std::array buffer{}; + EXPECT_EQ(XYParser_Serialize64GainSampleRateCommand(24, 256, buffer.data(), buffer.size()), 0); +} + +/// 测试：64 导阻抗命令在输出缓冲区不足时返回失败 +TEST(XYParserApiTests, Serialize64ImpedanceCommandRejectsSmallBuffer) +{ + std::array buffer{}; + EXPECT_EQ(XYParser_Serialize64ImpedanceCommand(1, buffer.data(), buffer.size()), 0); +} + // ============================================================================ // Feed 函数帧解析测试 // ============================================================================ diff --git a/XYParser/XYWelchProcessor.cpp b/XYParser/XYWelchProcessor.cpp new file mode 100644 index 0000000..03fab60 --- /dev/null +++ b/XYParser/XYWelchProcessor.cpp @@ -0,0 +1,285 @@ +#include "pch.h" +#include "XYWelchProcessor.h" + +#include "third_party/kissfft/kiss_fftr.h" +#include "third_party/kissfft/window_functions.h" + +#include +#include +#include +#include + +namespace { + +constexpr int kDefaultSampleRate = 250; +constexpr int k8ChLeadCount = 8; +constexpr int k64ChLeadCount = 64; + +struct PsdBandDefinition { + double low_hz; + double high_hz; +}; + +constexpr std::array k8ChLeadMap = { + LeadChannel_PO5, LeadChannel_POZ, LeadChannel_PO6, LeadChannel_PO7, + LeadChannel_O1, LeadChannel_OZ, LeadChannel_O2, LeadChannel_PO8}; + +constexpr std::array k64ChLeadMap = { + LeadChannel_FP1, LeadChannel_FP2, LeadChannel_PO6, LeadChannel_POZ, + LeadChannel_F3, LeadChannel_F4, LeadChannel_FPZ, LeadChannel_AF4, + LeadChannel_FC3, LeadChannel_PO8, LeadChannel_CP2, LeadChannel_CP1, + LeadChannel_FCZ, LeadChannel_PO5, LeadChannel_FC2, LeadChannel_FC1, + LeadChannel_C3, LeadChannel_C4, LeadChannel_FC4, LeadChannel_CP4, + LeadChannel_P3, LeadChannel_P4, LeadChannel_F5, LeadChannel_C5, + LeadChannel_F6, LeadChannel_PO4, LeadChannel_CP6, LeadChannel_CP5, + LeadChannel_PO3, LeadChannel_CP3, LeadChannel_FC6, LeadChannel_FC5, + LeadChannel_CB1, LeadChannel_CB2, LeadChannel_P5, LeadChannel_AF7, + LeadChannel_A1, LeadChannel_T7, LeadChannel_FT7, LeadChannel_TP7, + LeadChannel_FT8, LeadChannel_AF8, LeadChannel_F8, LeadChannel_F7, + LeadChannel_P6, LeadChannel_C6, LeadChannel_O2, LeadChannel_O1, + LeadChannel_T8, LeadChannel_P7, LeadChannel_CZ, LeadChannel_PZ, + LeadChannel_P8, LeadChannel_FZ, LeadChannel_OZ, LeadChannel_PO7, + LeadChannel_TP8, LeadChannel_AF3, LeadChannel_C2, LeadChannel_C1, + LeadChannel_P2, LeadChannel_P1, LeadChannel_F2, LeadChannel_F1}; + +constexpr std::array kWelchBands = {{ + {0.5, 4.0}, + {4.0, 8.0}, + {8.0, 13.0}, + {13.0, 30.0}, + {30.0, 50.0}, +}}; + +} // namespace + +XYWelchProcessor::XYWelchProcessor(std::uint8_t channel_count) + : channel_count_(channel_count) +{ +} + +void XYWelchProcessor::SetChannelCount(std::uint8_t channel_count) +{ + channel_count_ = channel_count; + Reset(); +} + +void XYWelchProcessor::SetSampleRate(int sample_rate) +{ + if (sample_rate <= 0) { + return; + } + + if (sample_rate_ != sample_rate) { + sample_rate_ = sample_rate; + Reset(); + } +} + +void XYWelchProcessor::SetEnabled(bool enabled) +{ + if (enabled_ == enabled) { + return; + } + + enabled_ = enabled; + Reset(); +} + +void XYWelchProcessor::Reset() +{ + cached_samples_.clear(); + pending_results_.clear(); +} + +void XYWelchProcessor::PushFrame(const XYParserFrameSummary& frame) +{ + if (!enabled_ || frame.channel_count != channel_count_) { + return; + } + + for (std::size_t sample_index = 0; sample_index < frame.sample_count; ++sample_index) { + Sample sample{}; + for (std::size_t channel_index = 0; channel_index < frame.channel_count; ++channel_index) { + sample[channel_index] = frame.channel_values_uv[sample_index][channel_index]; + } + cached_samples_.push_back(sample); + } + + ProcessPendingWindows(); +} + +void XYWelchProcessor::PushSample(const std::array& sample) +{ + if (!enabled_ || channel_count_ == 0) { + return; + } + + cached_samples_.push_back(sample); + ProcessPendingWindows(); +} + +int XYWelchProcessor::Read(XYParserWelchSummary* out_summaries, int max_summaries) +{ + if (out_summaries == nullptr || max_summaries <= 0) { + return 0; + } + + const int available_count = static_cast(pending_results_.size()); + const int read_count = available_count < max_summaries ? available_count : max_summaries; + for (int i = 0; i < read_count; ++i) { + out_summaries[i] = pending_results_.front(); + pending_results_.pop_front(); + } + return read_count; +} + +void XYWelchProcessor::ProcessPendingWindows() +{ + const int effective_sample_rate = sample_rate_ > 0 ? sample_rate_ : 1; + const std::size_t window_sample_count = static_cast(effective_sample_rate); + while (cached_samples_.size() >= window_sample_count) { + pending_results_.push_back(BuildSummaryFromWindow()); + cached_samples_.erase(cached_samples_.begin(), cached_samples_.begin() + static_cast(window_sample_count)); + } +} + +XYParserWelchSummary XYWelchProcessor::BuildSummaryFromWindow() const +{ + XYParserWelchSummary summary{}; + summary.channel_count = channel_count_; + summary.sample_rate = static_cast(sample_rate_ > 0 ? sample_rate_ : kDefaultSampleRate); + summary.window_sample_count = summary.sample_rate; + + const int sample_count = static_cast(summary.window_sample_count); + if (channel_count_ == 0 || sample_count < 2) { + return summary; + } + + int n_per_seg = sample_count; + if (n_per_seg % 2 != 0) { + --n_per_seg; + } + if (n_per_seg < 2) { + return summary; + } + + const int n_overlap = n_per_seg / 2; + const int n_step = (n_per_seg - n_overlap) > 0 ? (n_per_seg - n_overlap) : 1; + const int n_freq_bin_count = n_per_seg / 2 + 1; + + std::vector window(static_cast(n_per_seg), 0.0); + hanning_function(n_per_seg, window.data()); + + double window_power = 0.0; + for (int i = 0; i < n_per_seg; ++i) { + window_power += window[static_cast(i)] * window[static_cast(i)]; + } + if (window_power <= std::numeric_limits::epsilon()) { + return summary; + } + + std::vector> psd_accum(static_cast(channel_count_), + std::vector(static_cast(n_freq_bin_count), 0.0)); + std::vector segment(static_cast(n_per_seg), 0.0); + std::vector fft_output(static_cast(n_freq_bin_count)); + + kiss_fftr_cfg cfg = kiss_fftr_alloc(n_per_seg, 0, nullptr, nullptr); + if (cfg == nullptr) { + return summary; + } + + int segment_count = 0; + for (int start = 0; start + n_per_seg <= sample_count; start += n_step) { + ++segment_count; + for (std::size_t channel_index = 0; channel_index < channel_count_; ++channel_index) { + double mean = 0.0; + for (int i = 0; i < n_per_seg; ++i) { + mean += cached_samples_[static_cast(start + i)][channel_index]; + } + mean /= static_cast(n_per_seg); + + for (int i = 0; i < n_per_seg; ++i) { + segment[static_cast(i)] = + (cached_samples_[static_cast(start + i)][channel_index] - mean) * + window[static_cast(i)]; + } + + kiss_fftr(cfg, segment.data(), fft_output.data()); + for (int bin = 0; bin < n_freq_bin_count; ++bin) { + const double real = fft_output[static_cast(bin)].r; + const double imag = fft_output[static_cast(bin)].i; + double psd = (real * real + imag * imag) / + (static_cast(summary.sample_rate) * window_power); + if (bin != 0 && bin != (n_per_seg / 2)) { + psd *= 2.0; + } + psd_accum[channel_index][static_cast(bin)] += psd; + } + } + } + + kiss_fftr_free(cfg); + if (segment_count <= 0) { + return summary; + } + + std::vector all_frequencies(static_cast(n_freq_bin_count), 0.0); + const double freq_resolution = static_cast(summary.sample_rate) / static_cast(n_per_seg); + for (int bin = 0; bin < n_freq_bin_count; ++bin) { + all_frequencies[static_cast(bin)] = freq_resolution * static_cast(bin); + } + + for (int bin = 0; bin < n_freq_bin_count; ++bin) { + const double frequency = all_frequencies[static_cast(bin)]; + if (frequency >= 0.5 && frequency <= 50.0 && + summary.frequency_count < XYPARSER_WELCH_MAX_FREQUENCY_COUNT) { + summary.frequencies[summary.frequency_count] = frequency; + ++summary.frequency_count; + } + } + + const auto fill_channel_result = [&](std::size_t raw_channel_index, XYParserLeadChannelNumber lead) { + const std::size_t lead_index = static_cast(lead); + + std::uint32_t selected_index = 0; + for (int bin = 0; bin < n_freq_bin_count; ++bin) { + const double frequency = all_frequencies[static_cast(bin)]; + if (frequency >= 0.5 && frequency <= 50.0 && + selected_index < summary.frequency_count) { + summary.psd_values[lead_index][selected_index] = + psd_accum[raw_channel_index][static_cast(bin)] / static_cast(segment_count); + ++selected_index; + } + } + + for (std::size_t band_index = 0; band_index < kWelchBands.size(); ++band_index) { + int band_bin_count = 0; + double band_sum = 0.0; + for (int bin = 0; bin < n_freq_bin_count; ++bin) { + const double frequency = all_frequencies[static_cast(bin)]; + if (frequency < kWelchBands[band_index].low_hz || frequency > kWelchBands[band_index].high_hz) { + continue; + } + ++band_bin_count; + band_sum += psd_accum[raw_channel_index][static_cast(bin)] / static_cast(segment_count); + } + if (band_bin_count > 0) { + summary.band_values[band_index][lead_index] = band_sum / static_cast(band_bin_count); + } + } + }; + + if (channel_count_ == 8) { + for (std::size_t i = 0; i < k8ChLeadMap.size(); ++i) { + fill_channel_result(i, k8ChLeadMap[i]); + } + } else { + const std::size_t fill_count = channel_count_ < k64ChLeadMap.size() ? channel_count_ : k64ChLeadMap.size(); + for (std::size_t i = 0; i < fill_count; ++i) { + fill_channel_result(i, k64ChLeadMap[i]); + } + } + + summary.ok = 1; + return summary; +} diff --git a/XYParser/XYWelchProcessor.h b/XYParser/XYWelchProcessor.h new file mode 100644 index 0000000..d35900b --- /dev/null +++ b/XYParser/XYWelchProcessor.h @@ -0,0 +1,34 @@ +#pragma once + +#include "XYParserApi.h" + +#include +#include +#include + +class XYWelchProcessor { +public: + explicit XYWelchProcessor(std::uint8_t channel_count = 0); + + void SetChannelCount(std::uint8_t channel_count); + void SetSampleRate(int sample_rate); + void SetEnabled(bool enabled); + void Reset(); + + void PushFrame(const XYParserFrameSummary& frame); + void PushSample(const std::array& sample); + int Read(XYParserWelchSummary* out_summaries, int max_summaries); + +private: + using Sample = std::array; + + void ProcessPendingWindows(); + XYParserWelchSummary BuildSummaryFromWindow() const; + +private: + std::uint8_t channel_count_ = 0; + int sample_rate_ = 250; + bool enabled_ = false; + std::deque cached_samples_; + std::deque pending_results_; +}; diff --git a/XYParser/third_party/kissfft/_kiss_fft_guts.h b/XYParser/third_party/kissfft/_kiss_fft_guts.h new file mode 100644 index 0000000..eb7ed90 --- /dev/null +++ b/XYParser/third_party/kissfft/_kiss_fft_guts.h @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2003-2010, Mark Borgerding. All rights reserved. + * This file is part of KISS FFT - https://github.com/mborgerding/kissfft + * + * SPDX-License-Identifier: BSD-3-Clause + * See COPYING file for more information. + */ + +#ifndef _kiss_fft_guts_h +#define _kiss_fft_guts_h + +#include "kiss_fft.h" +#include "kiss_fft_log.h" + +#include + +#define MAXFACTORS 32 + +struct kiss_fft_state +{ + int nfft; + int inverse; + int factors[2 * MAXFACTORS]; + kiss_fft_cpx twiddles[1]; +}; + +#ifdef FIXED_POINT +#include +#if (FIXED_POINT == 32) +#define FRACBITS 31 +#define SAMPPROD int64_t +#define SAMP_MAX INT32_MAX +#define SAMP_MIN INT32_MIN +#else +#define FRACBITS 15 +#define SAMPPROD int32_t +#define SAMP_MAX INT16_MAX +#define SAMP_MIN INT16_MIN +#endif + +#if defined(CHECK_OVERFLOW) +#define CHECK_OVERFLOW_OP(a, op, b) \ + if((SAMPPROD)(a) op(SAMPPROD)(b) > SAMP_MAX || (SAMPPROD)(a) op(SAMPPROD)(b) < SAMP_MIN) { \ + KISS_FFT_WARNING("overflow (%d " #op " %d) = %ld", (a), (b), (SAMPPROD)(a) op(SAMPPROD)(b)); \ + } +#endif + +#define smul(a, b) ((SAMPPROD)(a) * (b)) +#define sround(x) (kiss_fft_scalar)(((x) + (1 << (FRACBITS - 1))) >> FRACBITS) +#define S_MUL(a, b) sround(smul(a, b)) + +#define C_MUL(m, a, b) \ + do { \ + (m).r = sround(smul((a).r, (b).r) - smul((a).i, (b).i)); \ + (m).i = sround(smul((a).r, (b).i) + smul((a).i, (b).r)); \ + } while(0) + +#define DIVSCALAR(x, k) ((x) = sround(smul(x, SAMP_MAX / (k)))) + +#define C_FIXDIV(c, div) \ + do { \ + DIVSCALAR((c).r, div); \ + DIVSCALAR((c).i, div); \ + } while(0) + +#define C_MULBYSCALAR(c, s) \ + do { \ + (c).r = sround(smul((c).r, s)); \ + (c).i = sround(smul((c).i, s)); \ + } while(0) + +#else + +#define S_MUL(a, b) ((a) * (b)) +#define C_MUL(m, a, b) \ + do { \ + (m).r = (a).r * (b).r - (a).i * (b).i; \ + (m).i = (a).r * (b).i + (a).i * (b).r; \ + } while(0) +#define C_FIXDIV(c, div) +#define C_MULBYSCALAR(c, s) \ + do { \ + (c).r *= (s); \ + (c).i *= (s); \ + } while(0) + +#endif + +#ifndef CHECK_OVERFLOW_OP +#define CHECK_OVERFLOW_OP(a, op, b) +#endif + +#define C_ADD(res, a, b) \ + do { \ + CHECK_OVERFLOW_OP((a).r, +, (b).r) \ + CHECK_OVERFLOW_OP((a).i, +, (b).i) \ + (res).r = (a).r + (b).r; \ + (res).i = (a).i + (b).i; \ + } while(0) + +#define C_SUB(res, a, b) \ + do { \ + CHECK_OVERFLOW_OP((a).r, -, (b).r) \ + CHECK_OVERFLOW_OP((a).i, -, (b).i) \ + (res).r = (a).r - (b).r; \ + (res).i = (a).i - (b).i; \ + } while(0) + +#define C_ADDTO(res, a) \ + do { \ + CHECK_OVERFLOW_OP((res).r, +, (a).r) \ + CHECK_OVERFLOW_OP((res).i, +, (a).i) \ + (res).r += (a).r; \ + (res).i += (a).i; \ + } while(0) + +#define C_SUBFROM(res, a) \ + do { \ + CHECK_OVERFLOW_OP((res).r, -, (a).r) \ + CHECK_OVERFLOW_OP((res).i, -, (a).i) \ + (res).r -= (a).r; \ + (res).i -= (a).i; \ + } while(0) + +#ifdef FIXED_POINT +#define KISS_FFT_COS(phase) floor(.5 + SAMP_MAX * cos(phase)) +#define KISS_FFT_SIN(phase) floor(.5 + SAMP_MAX * sin(phase)) +#define HALF_OF(x) ((x) >> 1) +#elif defined(USE_SIMD) +#define KISS_FFT_COS(phase) _mm_set1_ps(cos(phase)) +#define KISS_FFT_SIN(phase) _mm_set1_ps(sin(phase)) +#define HALF_OF(x) ((x) * _mm_set1_ps(.5)) +#else +#define KISS_FFT_COS(phase) (kiss_fft_scalar) cos(phase) +#define KISS_FFT_SIN(phase) (kiss_fft_scalar) sin(phase) +#define HALF_OF(x) ((x) * ((kiss_fft_scalar).5)) +#endif + +#define kf_cexp(x, phase) \ + do { \ + (x)->r = KISS_FFT_COS(phase); \ + (x)->i = KISS_FFT_SIN(phase); \ + } while(0) + +#define pcpx(c) KISS_FFT_DEBUG("%g + %gi\n", (double)((c)->r), (double)((c)->i)) + +#ifdef KISS_FFT_USE_ALLOCA +#include +#define KISS_FFT_TMP_ALLOC(nbytes) alloca(nbytes) +#define KISS_FFT_TMP_FREE(ptr) +#else +#define KISS_FFT_TMP_ALLOC(nbytes) KISS_FFT_MALLOC(nbytes) +#define KISS_FFT_TMP_FREE(ptr) KISS_FFT_FREE(ptr) +#endif + +#endif /* _kiss_fft_guts_h */ diff --git a/XYParser/third_party/kissfft/fir_filter.cpp b/XYParser/third_party/kissfft/fir_filter.cpp new file mode 100644 index 0000000..7d972bc --- /dev/null +++ b/XYParser/third_party/kissfft/fir_filter.cpp @@ -0,0 +1,186 @@ +#include "fir_filter.h" + +namespace { + +double sinc(const double x) +{ + if(x == 0.0) { + return 1.0; + } + return std::sin(M_PI * x) / (M_PI * x); +} + +} // namespace + +FIR_filter::FIR_filter(int taps, double f1, double f2, char *type, char *window) + : h(taps, 0.0), samples(taps, 0.0) +{ + this->idx = 0; + this->taps = taps; + + std::vector coeffs; + std::vector weights; + const char *safeType = type != nullptr ? type : const_cast(""); + const char *safeWindow = window != nullptr ? window : const_cast(""); + + if(std::strcmp(safeType, "lp") == 0) { + coeffs = lowPass_coefficient(taps, f1); + } else if(std::strcmp(safeType, "hp") == 0) { + coeffs = highPass_coefficient(taps, f1); + } else if(std::strcmp(safeType, "bp") == 0) { + coeffs = bandPass_coefficient(taps, f1, f2); + } else if(std::strcmp(safeType, "sb") == 0) { + coeffs = bandStop_coefficient(taps, f1, f2); + } + + if(std::strcmp(safeWindow, "hamming") == 0) { + weights = window_hammig(taps); + } else if(std::strcmp(safeWindow, "triangle") == 0) { + weights = window_triangle(taps); + } else if(std::strcmp(safeWindow, "hanning") == 0) { + weights = window_hanning(taps); + } else if(std::strcmp(safeWindow, "blackman") == 0) { + weights = window_blackman(taps); + } + + if(std::strcmp(safeWindow, "") == 0) { + this->h = coeffs; + } else { + for(int n = 0; n < taps; n++) { + this->h[n] = coeffs[n] * weights[n]; + } + } +} + +FIR_filter::~FIR_filter() = default; + +std::vector FIR_filter::getCoefficients() +{ + return this->h; +} + +std::vector FIR_filter::lowPass_coefficient(int taps, double f) +{ + std::vector n(taps, 0); + std::vector coeffs(taps, 0.0); + + for(int i = 0; i < taps; i++) { + n[i] = i - taps / 2; + } + for(int i = 0; i < taps; i++) { + coeffs[i] = 2.0 * f * sinc(2.0 * f * n[i]); + } + + return coeffs; +} + +std::vector FIR_filter::highPass_coefficient(int taps, double f) +{ + std::vector n(taps, 0); + std::vector coeffs(taps, 0.0); + + for(int i = 0; i < taps; i++) { + n[i] = i - taps / 2; + } + for(int i = 0; i < taps; i++) { + coeffs[i] = sinc(n[i]) - 2.0 * f * sinc(2.0 * f * n[i]); + } + + return coeffs; +} + +std::vector FIR_filter::bandPass_coefficient(int taps, double f1, double f2) +{ + std::vector n(taps, 0); + std::vector coeffs(taps, 0.0); + + for(int i = 0; i < taps; i++) { + n[i] = i - taps / 2; + } + for(int i = 0; i < taps; i++) { + coeffs[i] = 2.0 * f1 * sinc(2.0 * f1 * n[i]) - 2.0 * f2 * sinc(2.0 * f2 * n[i]); + } + + return coeffs; +} + +std::vector FIR_filter::bandStop_coefficient(int taps, double f1, double f2) +{ + std::vector n(taps, 0); + std::vector coeffs(taps, 0.0); + + for(int i = 0; i < taps; i++) { + n[i] = i - taps / 2; + } + for(int i = 0; i < taps; i++) { + coeffs[i] = 2.0 * f1 * sinc(2.0 * f1 * n[i]) - 2.0 * f2 * sinc(2.0 * f2 * n[i]) + + sinc(n[i]); + } + + return coeffs; +} + +std::vector FIR_filter::window_hammig(int taps) +{ + std::vector weights(taps, 0.0); + constexpr double alpha = 0.54; + constexpr double beta = 0.46; + + for(int i = 0; i < taps; i++) { + weights[i] = alpha - beta * std::cos(2.0 * M_PI * i / (taps - 1)); + } + + return weights; +} + +std::vector FIR_filter::window_hanning(int taps) +{ + std::vector weights(taps, 0.0); + + for(int i = 0; i < taps; i++) { + const double angle = static_cast(M_PI) * i / (taps - 1); + weights[i] = std::sin(angle) * std::sin(angle); + } + + return weights; +} + +std::vector FIR_filter::window_triangle(int taps) +{ + std::vector weights(taps, 0.0); + const double length = static_cast(taps); + + for(int i = 0; i < taps; i++) { + weights[i] = 1.0 - std::abs((i - ((taps - 1) / 2.0)) / (length / 2.0)); + } + + return weights; +} + +std::vector FIR_filter::window_blackman(int taps) +{ + std::vector weights(taps, 0.0); + constexpr double alpha0 = 0.42; + constexpr double alpha1 = 0.5; + constexpr double alpha2 = 0.08; + + for(int i = 0; i < taps; i++) { + weights[i] = alpha0 - alpha1 * std::cos(2.0 * M_PI * i / (taps - 1)) + - alpha2 * std::cos(4.0 * M_PI * i / (taps - 1)); + } + + return weights; +} + +double FIR_filter::filter(double new_sample) +{ + double result = 0.0; + + this->samples[this->idx] = new_sample; + for(int n = 0; n < this->taps; n++) { + result += this->samples[(this->idx + n) % this->taps] * this->h[n]; + } + this->idx = (this->idx + 1) % this->taps; + + return result; +} diff --git a/XYParser/third_party/kissfft/fir_filter.h b/XYParser/third_party/kissfft/fir_filter.h new file mode 100644 index 0000000..474e970 --- /dev/null +++ b/XYParser/third_party/kissfft/fir_filter.h @@ -0,0 +1,42 @@ +#ifndef FIR_FILTER_H +#define FIR_FILTER_H + +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +class FIR_filter +{ +public: + FIR_filter(int taps = 0, double f1 = 0, double f2 = 0, char *type = nullptr, + char *window = nullptr); + ~FIR_filter(); + + std::vector getCoefficients(); + + double filter(double new_sample); + +private: + std::vector lowPass_coefficient(int taps, double f); + std::vector highPass_coefficient(int taps, double f); + std::vector bandPass_coefficient(int taps, double f1, double f2); + std::vector bandStop_coefficient(int taps, double f1, double f2); + + std::vector window_hammig(int taps); + std::vector window_triangle(int taps); + std::vector window_hanning(int taps); + std::vector window_blackman(int taps); + + std::vector h; + std::vector samples; + + int idx = 0; + int taps = 0; +}; + +#endif // FIR_FILTER_H diff --git a/XYParser/third_party/kissfft/kiss_fft.c b/XYParser/third_party/kissfft/kiss_fft.c new file mode 100644 index 0000000..b815cd1 --- /dev/null +++ b/XYParser/third_party/kissfft/kiss_fft.c @@ -0,0 +1,349 @@ +/* + * Copyright (c) 2003-2010, Mark Borgerding. All rights reserved. + * This file is part of KISS FFT - https://github.com/mborgerding/kissfft + * + * SPDX-License-Identifier: BSD-3-Clause + * See COPYING file for more information. + */ + +#include "_kiss_fft_guts.h" + +static void kf_bfly2(kiss_fft_cpx *Fout, const size_t fstride, const kiss_fft_cfg st, int m) +{ + kiss_fft_cpx *Fout2 = Fout + m; + kiss_fft_cpx *tw1 = st->twiddles; + kiss_fft_cpx t; + + do { + C_FIXDIV(*Fout, 2); + C_FIXDIV(*Fout2, 2); + C_MUL(t, *Fout2, *tw1); + tw1 += fstride; + C_SUB(*Fout2, *Fout, t); + C_ADDTO(*Fout, t); + ++Fout2; + ++Fout; + } while(--m); +} + +static void kf_bfly4(kiss_fft_cpx *Fout, const size_t fstride, const kiss_fft_cfg st, + const size_t m) +{ + kiss_fft_cpx *tw1; + kiss_fft_cpx *tw2; + kiss_fft_cpx *tw3; + kiss_fft_cpx scratch[6]; + size_t k = m; + const size_t m2 = 2 * m; + const size_t m3 = 3 * m; + + tw3 = tw2 = tw1 = st->twiddles; + do { + C_FIXDIV(*Fout, 4); + C_FIXDIV(Fout[m], 4); + C_FIXDIV(Fout[m2], 4); + C_FIXDIV(Fout[m3], 4); + + C_MUL(scratch[0], Fout[m], *tw1); + C_MUL(scratch[1], Fout[m2], *tw2); + C_MUL(scratch[2], Fout[m3], *tw3); + + C_SUB(scratch[5], *Fout, scratch[1]); + C_ADDTO(*Fout, scratch[1]); + C_ADD(scratch[3], scratch[0], scratch[2]); + C_SUB(scratch[4], scratch[0], scratch[2]); + C_SUB(Fout[m2], *Fout, scratch[3]); + tw1 += fstride; + tw2 += fstride * 2; + tw3 += fstride * 3; + C_ADDTO(*Fout, scratch[3]); + + if(st->inverse) { + Fout[m].r = scratch[5].r - scratch[4].i; + Fout[m].i = scratch[5].i + scratch[4].r; + Fout[m3].r = scratch[5].r + scratch[4].i; + Fout[m3].i = scratch[5].i - scratch[4].r; + } else { + Fout[m].r = scratch[5].r + scratch[4].i; + Fout[m].i = scratch[5].i - scratch[4].r; + Fout[m3].r = scratch[5].r - scratch[4].i; + Fout[m3].i = scratch[5].i + scratch[4].r; + } + ++Fout; + } while(--k); +} + +static void kf_bfly3(kiss_fft_cpx *Fout, const size_t fstride, const kiss_fft_cfg st, size_t m) +{ + size_t k = m; + const size_t m2 = 2 * m; + kiss_fft_cpx *tw1 = st->twiddles; + kiss_fft_cpx *tw2 = st->twiddles; + kiss_fft_cpx scratch[5]; + kiss_fft_cpx epi3 = st->twiddles[fstride * m]; + + do { + C_FIXDIV(*Fout, 3); + C_FIXDIV(Fout[m], 3); + C_FIXDIV(Fout[m2], 3); + + C_MUL(scratch[1], Fout[m], *tw1); + C_MUL(scratch[2], Fout[m2], *tw2); + + C_ADD(scratch[3], scratch[1], scratch[2]); + C_SUB(scratch[0], scratch[1], scratch[2]); + tw1 += fstride; + tw2 += fstride * 2; + + Fout[m].r = Fout->r - HALF_OF(scratch[3].r); + Fout[m].i = Fout->i - HALF_OF(scratch[3].i); + + C_MULBYSCALAR(scratch[0], epi3.i); + C_ADDTO(*Fout, scratch[3]); + + Fout[m2].r = Fout[m].r + scratch[0].i; + Fout[m2].i = Fout[m].i - scratch[0].r; + Fout[m].r -= scratch[0].i; + Fout[m].i += scratch[0].r; + ++Fout; + } while(--k); +} + +static void kf_bfly5(kiss_fft_cpx *Fout, const size_t fstride, const kiss_fft_cfg st, int m) +{ + kiss_fft_cpx *Fout0 = Fout; + kiss_fft_cpx *Fout1 = Fout0 + m; + kiss_fft_cpx *Fout2 = Fout0 + 2 * m; + kiss_fft_cpx *Fout3 = Fout0 + 3 * m; + kiss_fft_cpx *Fout4 = Fout0 + 4 * m; + kiss_fft_cpx scratch[13]; + kiss_fft_cpx *tw = st->twiddles; + kiss_fft_cpx ya = st->twiddles[fstride * m]; + kiss_fft_cpx yb = st->twiddles[fstride * 2 * m]; + int u; + + for(u = 0; u < m; ++u) { + C_FIXDIV(*Fout0, 5); + C_FIXDIV(*Fout1, 5); + C_FIXDIV(*Fout2, 5); + C_FIXDIV(*Fout3, 5); + C_FIXDIV(*Fout4, 5); + scratch[0] = *Fout0; + + C_MUL(scratch[1], *Fout1, tw[u * fstride]); + C_MUL(scratch[2], *Fout2, tw[2 * u * fstride]); + C_MUL(scratch[3], *Fout3, tw[3 * u * fstride]); + C_MUL(scratch[4], *Fout4, tw[4 * u * fstride]); + + C_ADD(scratch[7], scratch[1], scratch[4]); + C_SUB(scratch[10], scratch[1], scratch[4]); + C_ADD(scratch[8], scratch[2], scratch[3]); + C_SUB(scratch[9], scratch[2], scratch[3]); + + Fout0->r += scratch[7].r + scratch[8].r; + Fout0->i += scratch[7].i + scratch[8].i; + + scratch[5].r = scratch[0].r + S_MUL(scratch[7].r, ya.r) + S_MUL(scratch[8].r, yb.r); + scratch[5].i = scratch[0].i + S_MUL(scratch[7].i, ya.r) + S_MUL(scratch[8].i, yb.r); + scratch[6].r = S_MUL(scratch[10].i, ya.i) + S_MUL(scratch[9].i, yb.i); + scratch[6].i = -S_MUL(scratch[10].r, ya.i) - S_MUL(scratch[9].r, yb.i); + + C_SUB(*Fout1, scratch[5], scratch[6]); + C_ADD(*Fout4, scratch[5], scratch[6]); + + scratch[11].r = scratch[0].r + S_MUL(scratch[7].r, yb.r) + S_MUL(scratch[8].r, ya.r); + scratch[11].i = scratch[0].i + S_MUL(scratch[7].i, yb.r) + S_MUL(scratch[8].i, ya.r); + scratch[12].r = -S_MUL(scratch[10].i, yb.i) + S_MUL(scratch[9].i, ya.i); + scratch[12].i = S_MUL(scratch[10].r, yb.i) - S_MUL(scratch[9].r, ya.i); + + C_ADD(*Fout2, scratch[11], scratch[12]); + C_SUB(*Fout3, scratch[11], scratch[12]); + + ++Fout0; + ++Fout1; + ++Fout2; + ++Fout3; + ++Fout4; + } +} + +static void kf_bfly_generic(kiss_fft_cpx *Fout, const size_t fstride, const kiss_fft_cfg st, + int m, int p) +{ + int u; + kiss_fft_cpx *twiddles = st->twiddles; + kiss_fft_cpx t; + int Norig = st->nfft; + kiss_fft_cpx *scratch = (kiss_fft_cpx *) KISS_FFT_TMP_ALLOC(sizeof(kiss_fft_cpx) * p); + + if(scratch == NULL) { + KISS_FFT_ERROR("Memory allocation failed."); + return; + } + + for(u = 0; u < m; ++u) { + int k = u; + int q1; + for(q1 = 0; q1 < p; ++q1) { + scratch[q1] = Fout[k]; + C_FIXDIV(scratch[q1], p); + k += m; + } + + k = u; + for(q1 = 0; q1 < p; ++q1) { + int q; + int twidx = 0; + Fout[k] = scratch[0]; + for(q = 1; q < p; ++q) { + twidx += (int) fstride * k; + if(twidx >= Norig) { + twidx -= Norig; + } + C_MUL(t, scratch[q], twiddles[twidx]); + C_ADDTO(Fout[k], t); + } + k += m; + } + } + KISS_FFT_TMP_FREE(scratch); +} + +static void kf_work(kiss_fft_cpx *Fout, const kiss_fft_cpx *f, const size_t fstride, + int in_stride, int *factors, const kiss_fft_cfg st) +{ + kiss_fft_cpx *Fout_beg = Fout; + const int p = *factors++; + const int m = *factors++; + const kiss_fft_cpx *Fout_end = Fout + p * m; + + if(m == 1) { + do { + *Fout = *f; + f += fstride * in_stride; + } while(++Fout != Fout_end); + } else { + do { + kf_work(Fout, f, fstride * p, in_stride, factors, st); + f += fstride * in_stride; + } while((Fout += m) != Fout_end); + } + + Fout = Fout_beg; + switch(p) { + case 2: kf_bfly2(Fout, fstride, st, m); break; + case 3: kf_bfly3(Fout, fstride, st, m); break; + case 4: kf_bfly4(Fout, fstride, st, m); break; + case 5: kf_bfly5(Fout, fstride, st, m); break; + default: kf_bfly_generic(Fout, fstride, st, m, p); break; + } +} + +static void kf_factor(int n, int *facbuf) +{ + int p = 4; + double floor_sqrt = floor(sqrt((double) n)); + + do { + while(n % p) { + switch(p) { + case 4: p = 2; break; + case 2: p = 3; break; + default: p += 2; break; + } + if(p > floor_sqrt) { + p = n; + } + } + n /= p; + *facbuf++ = p; + *facbuf++ = n; + } while(n > 1); +} + +kiss_fft_cfg kiss_fft_alloc(int nfft, int inverse_fft, void *mem, size_t *lenmem) +{ + KISS_FFT_ALIGN_CHECK(mem) + + kiss_fft_cfg st = NULL; + size_t memneeded = KISS_FFT_ALIGN_SIZE_UP(sizeof(struct kiss_fft_state) + + sizeof(kiss_fft_cpx) * (nfft - 1)); + + if(lenmem == NULL) { + st = (kiss_fft_cfg) KISS_FFT_MALLOC(memneeded); + } else { + if(mem != NULL && *lenmem >= memneeded) { + st = (kiss_fft_cfg) mem; + } + *lenmem = memneeded; + } + + if(st) { + int i; + st->nfft = nfft; + st->inverse = inverse_fft; + for(i = 0; i < nfft; ++i) { + const double pi = 3.141592653589793238462643383279502884197169399375105820974944; + double phase = -2 * pi * i / nfft; + if(st->inverse) { + phase *= -1; + } + kf_cexp(st->twiddles + i, phase); + } + kf_factor(nfft, st->factors); + } + return st; +} + +void kiss_fft_stride(kiss_fft_cfg st, const kiss_fft_cpx *fin, kiss_fft_cpx *fout, int in_stride) +{ + if(fin == fout) { + kiss_fft_cpx *tmpbuf; + if(fout == NULL) { + KISS_FFT_ERROR("fout buffer NULL."); + return; + } + + tmpbuf = (kiss_fft_cpx *) KISS_FFT_TMP_ALLOC(sizeof(kiss_fft_cpx) * st->nfft); + if(tmpbuf == NULL) { + KISS_FFT_ERROR("Memory allocation error."); + return; + } + + kf_work(tmpbuf, fin, 1, in_stride, st->factors, st); + memcpy(fout, tmpbuf, sizeof(kiss_fft_cpx) * st->nfft); + KISS_FFT_TMP_FREE(tmpbuf); + } else { + kf_work(fout, fin, 1, in_stride, st->factors, st); + } +} + +void kiss_fft(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, kiss_fft_cpx *fout) +{ + kiss_fft_stride(cfg, fin, fout, 1); +} + +void kiss_fft_cleanup(void) +{ +} + +int kiss_fft_next_fast_size(int n) +{ + while(1) { + int m = n; + while((m % 2) == 0) { + m /= 2; + } + while((m % 3) == 0) { + m /= 3; + } + while((m % 5) == 0) { + m /= 5; + } + if(m <= 1) { + break; + } + n++; + } + return n; +} diff --git a/XYParser/third_party/kissfft/kiss_fft.h b/XYParser/third_party/kissfft/kiss_fft.h new file mode 100644 index 0000000..abfa96b --- /dev/null +++ b/XYParser/third_party/kissfft/kiss_fft.h @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2003-2010, Mark Borgerding. All rights reserved. + * This file is part of KISS FFT - https://github.com/mborgerding/kissfft + * + * SPDX-License-Identifier: BSD-3-Clause + * See COPYING file for more information. + */ + +#ifndef KISS_FFT_H +#define KISS_FFT_H + +#include +#include +#include +#include + +#ifdef KISS_FFT_SHARED +#if defined(_WIN32) +#if defined(KISS_FFT_BUILD) +#define KISS_FFT_API __declspec(dllexport) +#else +#define KISS_FFT_API __declspec(dllimport) +#endif +#else +#define KISS_FFT_API __attribute__((visibility("default"))) +#endif +#else +#define KISS_FFT_API +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef USE_SIMD +#include +#define kiss_fft_scalar __m128 +#ifndef KISS_FFT_MALLOC +#define KISS_FFT_MALLOC(nbytes) _mm_malloc(nbytes, 16) +#define KISS_FFT_ALIGN_CHECK(ptr) +#define KISS_FFT_ALIGN_SIZE_UP(size) ((size + 15UL) & ~0xFUL) +#endif +#ifndef KISS_FFT_FREE +#define KISS_FFT_FREE _mm_free +#endif +#else +#define KISS_FFT_ALIGN_CHECK(ptr) +#define KISS_FFT_ALIGN_SIZE_UP(size) (size) +#ifndef KISS_FFT_MALLOC +#define KISS_FFT_MALLOC malloc +#endif +#ifndef KISS_FFT_FREE +#define KISS_FFT_FREE free +#endif +#endif + +#ifdef FIXED_POINT +#include +#if (FIXED_POINT == 32) +#define kiss_fft_scalar int32_t +#else +#define kiss_fft_scalar int16_t +#endif +#else +#ifndef kiss_fft_scalar +#define kiss_fft_scalar double +#endif +#endif + +typedef struct { + kiss_fft_scalar r; + kiss_fft_scalar i; +} kiss_fft_cpx; + +typedef struct kiss_fft_state *kiss_fft_cfg; + +kiss_fft_cfg KISS_FFT_API kiss_fft_alloc(int nfft, int inverse_fft, void *mem, size_t *lenmem); +void KISS_FFT_API kiss_fft(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, kiss_fft_cpx *fout); +void KISS_FFT_API kiss_fft_stride(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, kiss_fft_cpx *fout, + int fin_stride); + +#define kiss_fft_free KISS_FFT_FREE + +void KISS_FFT_API kiss_fft_cleanup(void); +int KISS_FFT_API kiss_fft_next_fast_size(int n); + +#define kiss_fftr_next_fast_size_real(n) (kiss_fft_next_fast_size(((n) + 1) >> 1) << 1) + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/XYParser/third_party/kissfft/kiss_fft_log.h b/XYParser/third_party/kissfft/kiss_fft_log.h new file mode 100644 index 0000000..b35cc8b --- /dev/null +++ b/XYParser/third_party/kissfft/kiss_fft_log.h @@ -0,0 +1,34 @@ +/* + * Copyright (c) 2003-2010, Mark Borgerding. All rights reserved. + * This file is part of KISS FFT - https://github.com/mborgerding/kissfft + * + * SPDX-License-Identifier: BSD-3-Clause + * See COPYING file for more information. + */ + +#ifndef kiss_fft_log_h +#define kiss_fft_log_h + +#define ERROR 1 +#define WARNING 2 +#define INFO 3 +#define DEBUG 4 + +#define STRINGIFY(x) #x +#define TOSTRING(x) STRINGIFY(x) + +#if defined(NDEBUG) +#define KISS_FFT_LOG_MSG(severity, ...) ((void)0) +#else +#define KISS_FFT_LOG_MSG(severity, ...) \ + fprintf(stderr, "[" #severity "] " __FILE__ ":" TOSTRING(__LINE__) " "); \ + fprintf(stderr, __VA_ARGS__); \ + fprintf(stderr, "\n") +#endif + +#define KISS_FFT_ERROR(...) KISS_FFT_LOG_MSG(ERROR, __VA_ARGS__) +#define KISS_FFT_WARNING(...) KISS_FFT_LOG_MSG(WARNING, __VA_ARGS__) +#define KISS_FFT_INFO(...) KISS_FFT_LOG_MSG(INFO, __VA_ARGS__) +#define KISS_FFT_DEBUG(...) KISS_FFT_LOG_MSG(DEBUG, __VA_ARGS__) + +#endif /* kiss_fft_log_h */ diff --git a/XYParser/third_party/kissfft/kiss_fftr.c b/XYParser/third_party/kissfft/kiss_fftr.c new file mode 100644 index 0000000..2ec1de5 --- /dev/null +++ b/XYParser/third_party/kissfft/kiss_fftr.c @@ -0,0 +1,159 @@ +/* + * Copyright (c) 2003-2004, Mark Borgerding. All rights reserved. + * This file is part of KISS FFT - https://github.com/mborgerding/kissfft + * + * SPDX-License-Identifier: BSD-3-Clause + * See COPYING file for more information. + */ + +#include "kiss_fftr.h" +#include "_kiss_fft_guts.h" + +struct kiss_fftr_state +{ + kiss_fft_cfg substate; + kiss_fft_cpx *tmpbuf; + kiss_fft_cpx *super_twiddles; +#ifdef USE_SIMD + void *pad; +#endif +}; + +kiss_fftr_cfg kiss_fftr_alloc(int nfft, int inverse_fft, void *mem, size_t *lenmem) +{ + KISS_FFT_ALIGN_CHECK(mem) + + int i; + kiss_fftr_cfg st = NULL; + size_t subsize = 0; + size_t memneeded; + + if(nfft & 1) { + KISS_FFT_ERROR("Real FFT optimization must be even."); + return NULL; + } + nfft >>= 1; + + kiss_fft_alloc(nfft, inverse_fft, NULL, &subsize); + memneeded = sizeof(struct kiss_fftr_state) + subsize + sizeof(kiss_fft_cpx) * (nfft * 3 / 2); + + if(lenmem == NULL) { + st = (kiss_fftr_cfg) KISS_FFT_MALLOC(memneeded); + } else { + if(*lenmem >= memneeded) { + st = (kiss_fftr_cfg) mem; + } + *lenmem = memneeded; + } + if(!st) { + return NULL; + } + + st->substate = (kiss_fft_cfg) (st + 1); + st->tmpbuf = (kiss_fft_cpx *) (((char *) st->substate) + subsize); + st->super_twiddles = st->tmpbuf + nfft; + kiss_fft_alloc(nfft, inverse_fft, st->substate, &subsize); + + for(i = 0; i < nfft / 2; ++i) { + double phase = -3.14159265358979323846264338327 * ((double) (i + 1) / nfft + .5); + if(inverse_fft) { + phase *= -1; + } + kf_cexp(st->super_twiddles + i, phase); + } + + return st; +} + +void kiss_fftr(kiss_fftr_cfg st, const kiss_fft_scalar *timedata, kiss_fft_cpx *freqdata) +{ + int k; + int ncfft; + kiss_fft_cpx fpnk; + kiss_fft_cpx fpk; + kiss_fft_cpx f1k; + kiss_fft_cpx f2k; + kiss_fft_cpx tw; + kiss_fft_cpx tdc; + + if(st->substate->inverse) { + KISS_FFT_ERROR("kiss fft usage error: improper alloc"); + return; + } + + ncfft = st->substate->nfft; + kiss_fft(st->substate, (const kiss_fft_cpx *) timedata, st->tmpbuf); + + tdc.r = st->tmpbuf[0].r; + tdc.i = st->tmpbuf[0].i; + C_FIXDIV(tdc, 2); + CHECK_OVERFLOW_OP(tdc.r, +, tdc.i); + CHECK_OVERFLOW_OP(tdc.r, -, tdc.i); + freqdata[0].r = tdc.r + tdc.i; + freqdata[ncfft].r = tdc.r - tdc.i; +#ifdef USE_SIMD + freqdata[ncfft].i = freqdata[0].i = _mm_set1_ps(0); +#else + freqdata[ncfft].i = freqdata[0].i = 0; +#endif + + for(k = 1; k <= ncfft / 2; ++k) { + fpk = st->tmpbuf[k]; + fpnk.r = st->tmpbuf[ncfft - k].r; + fpnk.i = -st->tmpbuf[ncfft - k].i; + C_FIXDIV(fpk, 2); + C_FIXDIV(fpnk, 2); + + C_ADD(f1k, fpk, fpnk); + C_SUB(f2k, fpk, fpnk); + C_MUL(tw, f2k, st->super_twiddles[k - 1]); + + freqdata[k].r = HALF_OF(f1k.r + tw.r); + freqdata[k].i = HALF_OF(f1k.i + tw.i); + freqdata[ncfft - k].r = HALF_OF(f1k.r - tw.r); + freqdata[ncfft - k].i = HALF_OF(tw.i - f1k.i); + } +} + +void kiss_fftri(kiss_fftr_cfg st, const kiss_fft_cpx *freqdata, kiss_fft_scalar *timedata) +{ + int k; + int ncfft; + + if(st->substate->inverse == 0) { + KISS_FFT_ERROR("kiss fft usage error: improper alloc"); + return; + } + + ncfft = st->substate->nfft; + st->tmpbuf[0].r = freqdata[0].r + freqdata[ncfft].r; + st->tmpbuf[0].i = freqdata[0].r - freqdata[ncfft].r; + C_FIXDIV(st->tmpbuf[0], 2); + + for(k = 1; k <= ncfft / 2; ++k) { + kiss_fft_cpx fk; + kiss_fft_cpx fnkc; + kiss_fft_cpx fek; + kiss_fft_cpx fok; + kiss_fft_cpx tmp; + + fk = freqdata[k]; + fnkc.r = freqdata[ncfft - k].r; + fnkc.i = -freqdata[ncfft - k].i; + C_FIXDIV(fk, 2); + C_FIXDIV(fnkc, 2); + + C_ADD(fek, fk, fnkc); + C_SUB(tmp, fk, fnkc); + C_MUL(fok, tmp, st->super_twiddles[k - 1]); + C_ADD(st->tmpbuf[k], fek, fok); + C_SUB(st->tmpbuf[ncfft - k], fek, fok); +#ifdef USE_SIMD + st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0); +#else + st->tmpbuf[ncfft - k].i *= -1; +#endif + } + + kiss_fft(st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata); +} diff --git a/XYParser/third_party/kissfft/kiss_fftr.h b/XYParser/third_party/kissfft/kiss_fftr.h new file mode 100644 index 0000000..54e0595 --- /dev/null +++ b/XYParser/third_party/kissfft/kiss_fftr.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) 2003-2004, Mark Borgerding. All rights reserved. + * This file is part of KISS FFT - https://github.com/mborgerding/kissfft + * + * SPDX-License-Identifier: BSD-3-Clause + * See COPYING file for more information. + */ + +#ifndef KISS_FTR_H +#define KISS_FTR_H + +#include "kiss_fft.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kiss_fftr_state *kiss_fftr_cfg; + +kiss_fftr_cfg KISS_FFT_API kiss_fftr_alloc(int nfft, int inverse_fft, void *mem, size_t *lenmem); +void KISS_FFT_API kiss_fftr(kiss_fftr_cfg cfg, const kiss_fft_scalar *timedata, + kiss_fft_cpx *freqdata); +void KISS_FFT_API kiss_fftri(kiss_fftr_cfg cfg, const kiss_fft_cpx *freqdata, + kiss_fft_scalar *timedata); + +#define kiss_fftr_free KISS_FFT_FREE + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/XYParser/third_party/kissfft/window_functions.h b/XYParser/third_party/kissfft/window_functions.h new file mode 100644 index 0000000..b7d5850 --- /dev/null +++ b/XYParser/third_party/kissfft/window_functions.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +inline void no_window_function(int window_len, double *wind) +{ + for(int i = 0; i < window_len; i++) { + wind[i] = 1.0; + } +} + +inline void hanning_function(int window_len, double *wind) +{ + for(int i = 0; i < window_len; i++) { + wind[i] = 0.5 - 0.5 * std::cos(2.0 * M_PI * i / window_len); + } +} + +inline void hamming_function(int window_len, double *wind) +{ + for(int i = 0; i < window_len; i++) { + wind[i] = 0.54 - 0.46 * std::cos(2.0 * M_PI * i / window_len); + } +} + +inline void blackman_harris_function(int window_len, double *wind) +{ + for(int i = 0; i < window_len; i++) { + wind[i] = 0.355768 - 0.487396 * std::cos(2.0 * M_PI * i / window_len) + + 0.144232 * std::cos(4.0 * M_PI * i / window_len) + - 0.012604 * std::cos(6.0 * M_PI * i / window_len); + } +}