From f1e84f5f99f42f6e810a77611cf197826a430271 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 7 Jul 2024 11:22:42 +0800 Subject: [PATCH] init --- a8k_opt_algo.cpp | 406 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ a8k_opt_algo.hpp | 68 ++++++++++ build_pc.bat | 1 + 3 files changed, 475 insertions(+) create mode 100644 a8k_opt_algo.cpp create mode 100644 a8k_opt_algo.hpp create mode 100644 build_pc.bat diff --git a/a8k_opt_algo.cpp b/a8k_opt_algo.cpp new file mode 100644 index 0000000..6f62add --- /dev/null +++ b/a8k_opt_algo.cpp @@ -0,0 +1,406 @@ +#include "a8k_opt_algo.hpp" + +namespace a8k_opt_algo { +using namespace std; + +typedef enum { + ktopt, + kfopt, +} opt_type_t; + +class PorcessContext { + public: + vector raw; // + vector avg; // + vector diff; // 一阶斜率 + vector diffX2; // 二阶斜率 + float agvline; // +}; + +static void algo_assert(bool condition, const char* msg) { + if (!condition) { + printf("algo_assert:%s\n", msg); + throw std::runtime_error(msg); + } +} +bool feq(float a, float b, float epsilon) { + float dv = a - b; + if (dv < 0) dv = -dv; + return dv <= epsilon; +} + +opt_type_t m_opttype; +PorcessContext m_cxt; + +static vector sub_sampling(vector& inputRaw, int nSubSampleRate); +vector super_sampling(vector& inputRaw, int32_t nInputLength, int32_t nUpSampleRate); + +static vector sub_sampling(vector& inputRaw, int nSubSampleRate) { + int nSum = 0; + float fAvg = 0; + int subIndex = 0; + int nOutputLength = inputRaw.size() / nSubSampleRate; + + vector subSampledRaw(nOutputLength, 0); + + for (int index = 0; index < inputRaw.size(); index++) { + if (index % nSubSampleRate == 0 && index > 0) { + fAvg = nSum / nSubSampleRate; + if (subIndex < subSampledRaw.size()) { + subSampledRaw[subIndex++] = fAvg; + } else { + int empty = 0; + } + nSum = 0; + } + nSum += inputRaw[index]; + } + subSampledRaw[subSampledRaw.size() - 1] = subSampledRaw[subSampledRaw.size() - 2]; + return subSampledRaw; +} + +vector super_sampling(vector& inputRaw, int32_t nInputLength, int32_t nUpSampleRate) { + /** + * @brief + * + */ + int nOutputLength = nInputLength * nUpSampleRate; + vector upSamplingRaw(nOutputLength, 0); + + for (int si = 0, di = 0; si < nInputLength - 1; di++) { + float a = upSamplingRaw[di * nUpSampleRate] = (float)inputRaw[si]; + float b = upSamplingRaw[(di + 1) * nUpSampleRate] = (float)inputRaw[++si]; + + float nSlope = (b - a) / nUpSampleRate; + + for (int i = 0; i < nUpSampleRate - 1; i++) { + int baseIndex = (di * nUpSampleRate) + i; + upSamplingRaw[baseIndex + 1] = upSamplingRaw[baseIndex] + nSlope; + } + } + + return upSamplingRaw; +} + +vector getwindowspoint(vector& src, int off, int windows) { + vector ret(windows, 0); + int retindex = 0; + for (int i = off - windows / 2; i <= off + windows / 2; i++) { + ret[retindex] = src[i]; + retindex++; + } + return ret; +} + +/** + * @brief 最小二乘法求解曲线斜率 + * + * @param val Y轴数据 + * @param size Y轴数据长度 + * @return float 斜率 + */ + +void linear_least_squares(vector& x, vector& y, float& slope, float& intercept) { + size_t n = x.size(); + double sumX = 0.0, sumY = 0.0, sumXY = 0.0, sumXX = 0.0; + for (size_t i = 0; i < n; ++i) { + sumX += x[i]; + sumY += y[i]; + sumXY += x[i] * y[i]; + sumXX += x[i] * x[i]; + } + double xMean = sumX / n; + double yMean = sumY / n; + + algo_assert(!feq((sumXX - n * xMean * xMean), 0, 0.0001), "sumXX - n * xMean * xMean == 0"); + slope = (sumXY - n * xMean * yMean) / (sumXX - n * xMean * xMean); + intercept = yMean - slope * xMean; + return; +} + +void linear_least_squares(float* y, int size, float& slope, float& intercept) { + vector xpoint(size, 0); + vector ypoint(size, 0); + + for (size_t i = 0; i < size; i++) { + xpoint[i] = i; + ypoint[i] = y[i]; + } + return linear_least_squares(xpoint, ypoint, slope, intercept); +} + +void linear_least_squares_muti_windos(float* y, int size, vector startx, int windowssize, float& slope, float& intercept) { + vector xpoint; + vector ypoint; + + // ZLOGI(TAG, "xxxxx%d", startx.size()); + + for (size_t i = 0; i < startx.size(); i++) { + int xstart = startx[i]; + + for (size_t xindex = xstart; xindex < (xstart + windowssize); xindex++) { + // ZLOGI(TAG, "xindex:%d y:%f", xindex, y[xindex]); + xpoint.push_back(xindex); + ypoint.push_back(y[xindex]); + } + } + return linear_least_squares(xpoint, ypoint, slope, intercept); +} + +vector least_square_method_differentiate(vector& inputRaw, int windows_size) { + algo_assert(windows_size > 0, "windows_size <= 0"); + algo_assert(windows_size % 2 == 1, "windows_size is not odd"); + + vector differentiateRaw(inputRaw.size(), 0); + vector windowsRaw(windows_size, 0); + int windows_size_half = (windows_size - 1) / 2; + + for (int index = windows_size_half; index < inputRaw.size() - windows_size_half; index++) { + windowsRaw = getwindowspoint(inputRaw, index, windows_size); + float intercept = 0; + linear_least_squares(windowsRaw.data(), windows_size, differentiateRaw[index], intercept); + } + + for (size_t i = 0; i < windows_size_half; i++) { + differentiateRaw[i] = differentiateRaw[windows_size_half]; + } + + for (size_t i = inputRaw.size() - windows_size_half; i < inputRaw.size(); i++) { + differentiateRaw[i] = differentiateRaw[inputRaw.size() - windows_size_half - 1]; + } + return differentiateRaw; +} +vector smooth_windows(vector& inputRaw, int windows_size) { + vector smoothRaw(inputRaw.size(), 0); + int windows_size_half = (windows_size - 1) / 2; + + for (int index = windows_size_half; index < inputRaw.size() - windows_size_half; index++) { + float sum = 0; + for (int i = index - windows_size_half; i <= index + windows_size_half; i++) { + sum += inputRaw[i]; + } + smoothRaw[index] = sum / windows_size; + } + + for (size_t i = 0; i < windows_size_half; i++) { + smoothRaw[i] = smoothRaw[windows_size_half]; + } + + for (size_t i = inputRaw.size() - windows_size_half; i < inputRaw.size(); i++) { + smoothRaw[i] = smoothRaw[inputRaw.size() - windows_size_half - 1]; + } + + return smoothRaw; +} + +float find_avg_line(vector& inputRaw) { + float base_min = 500; + float fsum = 0; + int cnt = 0; + + int range = inputRaw.size(); + + do { + fsum = cnt = 0; + for (int i = 1; i < range; i++) { + if (inputRaw[i] < base_min) { + fsum += inputRaw[i]; + cnt++; + } + } + + base_min = base_min + 50; + } while (cnt < range - 15 * inputRaw.size() / 250); + + float fbase = fsum / cnt; + return fbase; +} + +/*********************************************************************************************************************** + * ALGO_IMPL * + ***********************************************************************************************************************/ + +int32_t findPeakTurnPoint(vector& data, int32_t search_start, int32_t suggest_search_end) { + int32_t search_end = 0; + for (int32_t i = search_start; i < suggest_search_end; i++) { + if (data[i] > m_cxt.agvline) { + search_end = i; + } + } + + int32_t peakTurnPos = 0; + float maxdiff2 = 0; + for (int32_t i = search_start; i < search_end; i++) { + if (m_cxt.diffX2[i] > maxdiff2) { + maxdiff2 = m_cxt.diffX2[i]; + peakTurnPos = i; + } + } + return peakTurnPos; +} + +float computePeakArea(vector& data, int32_t start, int32_t end) { + float area = 0; + for (int i = start; i < end; i++) { + area += data[i]; + } + + float baselinearea = 0; + baselinearea = (data[start] + data[end]) * abs(end - start) / 2; + + return abs(area - baselinearea); +} + +void findpeak(vector& data, int32_t search_start, int32_t search_end, PeakInfo& retpeak) { + // find peak + float max = 0; + int peakpos = 0; + float midpos = (search_end - search_start) / 2; + for (int i = search_start; i < search_end; i++) { + if (data[i] > max) { + max = data[i]; + peakpos = i; + } + } + + if (max < m_cxt.agvline) { + retpeak.find_peak = false; + return; + } else if (peakpos > midpos + 10) { + retpeak.find_peak = false; + return; + } else if (peakpos < midpos - 10) { + retpeak.find_peak = false; + return; + } + + // find_peak_start + // 从pos向前找20个点，从低于均值线的坐标开始找，找到diff2的最大值 + retpeak.peak_pos = peakpos; + retpeak.peak_start_pos = findPeakTurnPoint(data, peakpos - 20, peakpos); + retpeak.peak_end_pos = findPeakTurnPoint(data, peakpos, peakpos + 20); + retpeak.area = computePeakArea(data, retpeak.peak_start_pos, retpeak.peak_end_pos); + + // find_peak_end + // 从pos向后找20个点，找到diff2的最大值 +} + +void a8k_opt_algo_process(vector& ogigin_val, OptAlgoResult& result) { + // + + vector super = super_sampling(ogigin_val, ogigin_val.size(), 5); + vector subsample = sub_sampling(ogigin_val, 24); + + m_cxt.raw = subsample; + m_cxt.avg = smooth_windows(subsample, 4); + m_cxt.diff = least_square_method_differentiate(m_cxt.avg, 4); // 最小二乘法求曲线斜率集合 + m_cxt.diffX2 = least_square_method_differentiate(m_cxt.diff, 4); // 最小二乘法求曲线二次斜率集合 + m_cxt.agvline = find_avg_line(subsample); + // findPeak + + for (size_t i = 0; i < 5; i++) { + findpeak(m_cxt.avg, 20, 60, result.pin040); + findpeak(m_cxt.avg, 60, 100, result.pin080); + findpeak(m_cxt.avg, 100, 140, result.pin120); + findpeak(m_cxt.avg, 140, 180, result.pin160); + findpeak(m_cxt.avg, 180, 220, result.pin200); + } +} + +void A8kOptAlgoProcess(vector ogigin_val, OptAlgoResult& result) { // + a8k_opt_algo_process(ogigin_val, result); +} + +/*********************************************************************************************************************** + * T_A8kOptAlgoPreProcess * + ***********************************************************************************************************************/ +int32_t t_detector_gain_to_raw_gain(float scan_gain) { + // opamp_gain = (((100.0 * (float) scan_gain_raw) / 255) + 2.4) / 4.7; + int32_t scan_gain_raw = 0; + scan_gain_raw = (scan_gain * 4.7 - 2.4) * 256. / 100. + 0.5; + if (scan_gain_raw < 1) scan_gain_raw = 1; + if (scan_gain_raw > 255) scan_gain_raw = 255; + return scan_gain_raw; +} +float t_detector_raw_gain_to_gain(int32_t gain) { + float scan_gain = 0; + scan_gain = (((100.0 * (float)gain) / 256) + 2.4) / 4.7; + return scan_gain; +} + +void T_A8kOptAlgoPreProcess(vector ogigin_val, int32_t now_scan_gain, int32_t expectResultRangeStart, int32_t expectResultRangeEnd, OptAlgoPreProcessResult& result) { + int32_t adcgoal = expectResultRangeEnd; + int32_t maxadc = ogigin_val[0]; + + result.scanAgain = false; + result.suggestScanGain = now_scan_gain; + + for (int32_t i = 1; i < ogigin_val.size(); i++) { + if (maxadc < ogigin_val[i]) { + maxadc = ogigin_val[i]; + } + } + + if (maxadc > expectResultRangeStart) { + result.scanAgain = false; + return; + } + + float nowgain = t_detector_raw_gain_to_gain(now_scan_gain); + float gain_adjust = 0; + if (maxadc != 0) { + gain_adjust = nowgain * ((float)adcgoal / (float)maxadc); + } else { + gain_adjust = nowgain; + } + result.suggestScanGain = t_detector_gain_to_raw_gain(gain_adjust); + result.scanAgain = true; +} + + +/*********************************************************************************************************************** + * F_A8kOptAlgoPreProcess * + ***********************************************************************************************************************/ +float f_detector_raw_gain_to_gain(int32_t gain) { + float scan_gain = 0; + scan_gain = (((100. / 256.) * (float)gain) + 0.125) / 2.15 + 1.; + return scan_gain; +} +int32_t f_detector_gain_to_raw_gain(float scan_gain) { + // int32_t scan_gain = (((100. / 256.) * (float)scan_gain_raw) + 0.125) / 2.15 + 1.; + int32_t scan_gain_raw = 0; + scan_gain_raw = ((scan_gain - 1.0) * 2.15 - 0.125) * 255. / 100. + 0.5; + if (scan_gain_raw < 1) scan_gain_raw = 1; + if (scan_gain_raw > 255) scan_gain_raw = 255; + return scan_gain_raw; +} +void F_A8kOptAlgoPreProcess(vector ogigin_val, int32_t now_scan_gain, int32_t expectResultRangeStart, int32_t expectResultRangeEnd, OptAlgoPreProcessResult& result) { + int32_t adcgoal = expectResultRangeEnd; + int32_t maxadc = ogigin_val[0]; + + result.scanAgain = false; + result.suggestScanGain = now_scan_gain; + + for (int32_t i = 1; i < ogigin_val.size(); i++) { + if (maxadc < ogigin_val[i]) { + maxadc = ogigin_val[i]; + } + } + + if (maxadc > expectResultRangeStart) { + result.scanAgain = false; + return; + } + + float nowgain = f_detector_raw_gain_to_gain(now_scan_gain); + float gain_adjust = 0; + if (maxadc != 0) { + gain_adjust = nowgain * ((float)adcgoal / (float)maxadc); + } else { + gain_adjust = nowgain; + } + result.suggestScanGain = f_detector_gain_to_raw_gain(gain_adjust); + result.scanAgain = true; +} + +} // namespace a8k_opt_algo \ No newline at end of file diff --git a/a8k_opt_algo.hpp b/a8k_opt_algo.hpp new file mode 100644 index 0000000..ae81c54 --- /dev/null +++ b/a8k_opt_algo.hpp @@ -0,0 +1,68 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace a8k_opt_algo { +using namespace std; + +/*********************************************************************************************************************** + * 预处理 * + ***********************************************************************************************************************/ + +class OptAlgoPreProcessResult { + public: + bool scanAgain; + int32_t suggestScanGain; +}; + +void T_A8kOptAlgoPreProcess(vector ogigin_val, // + int32_t now_scan_gain, // + int32_t expectResultRangeStart, // + int32_t expectResultRangeEnd, // + OptAlgoPreProcessResult& result); +void F_A8kOptAlgoPreProcess(vector ogigin_val, // + int32_t now_scan_gain, // + int32_t expectResultRangeStart, // + int32_t expectResultRangeEnd, // + OptAlgoPreProcessResult& result); + +/*********************************************************************************************************************** + * 数据分析 * + ***********************************************************************************************************************/ +class PeakInfo { + public: + bool find_peak; // 是否找到峰 + float area; // 峰面积 + int peak_pos; // 峰位置 + int peak_start_pos; // 峰开始位置 + int peak_end_pos; // 峰结束位置 +}; + +class OptAlgoResult { + public: + /** + * @brief 原始数据 + */ + vector displayData; // 250 压缩后的数据 + + /** + * @brief 峰的信息 + */ + PeakInfo pin040; // 峰040的信息 + PeakInfo pin080; // 峰080的信息 + PeakInfo pin120; // 峰120的信息 + PeakInfo pin160; // 峰160的信息 + PeakInfo pin200; // 峰200的信息 +}; + +void A8kOptAlgoProcess(vector ogigin_val, OptAlgoResult& result); + +} // namespace a8k_opt_algo \ No newline at end of file diff --git a/build_pc.bat b/build_pc.bat new file mode 100644 index 0000000..b2d7df0 --- /dev/null +++ b/build_pc.bat @@ -0,0 +1 @@ +g++ a8k_opt_algo.cpp -fPIC -shared -o liba8k_opt_algo.so \ No newline at end of file