init

1 year ago · f1e84f5f99
3 changed files with 475 additions and 0 deletions
--- a/a8k_opt_algo.cpp
+++ 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<float> raw;      //
+  vector<float> avg;      //
+  vector<float> diff;     // 一阶斜率
+  vector<float> 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<float> sub_sampling(vector<float>& inputRaw, int nSubSampleRate);
+vector<float>        super_sampling(vector<float>& inputRaw, int32_t nInputLength, int32_t nUpSampleRate);
+
+static vector<float> sub_sampling(vector<float>& inputRaw, int nSubSampleRate) {
+  int   nSum          = 0;
+  float fAvg          = 0;
+  int   subIndex      = 0;
+  int   nOutputLength = inputRaw.size() / nSubSampleRate;
+
+  vector<float> 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<float> super_sampling(vector<float>& inputRaw, int32_t nInputLength, int32_t nUpSampleRate) {
+  /**
+   * @brief
+   *
+   */
+  int           nOutputLength = nInputLength * nUpSampleRate;
+  vector<float> 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<float> getwindowspoint(vector<float>& src, int off, int windows) {
+  vector<float> 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<float>& x, vector<float>& 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<float> xpoint(size, 0);
+  vector<float> 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<int> startx, int windowssize, float& slope, float& intercept) {
+  vector<float> xpoint;
+  vector<float> 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<float> least_square_method_differentiate(vector<float>& inputRaw, int windows_size) {
+  algo_assert(windows_size > 0, "windows_size <= 0");
+  algo_assert(windows_size % 2 == 1, "windows_size is not odd");
+
+  vector<float> differentiateRaw(inputRaw.size(), 0);
+  vector<float> 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<float> smooth_windows(vector<float>& inputRaw, int windows_size) {
+  vector<float> 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<float>& 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<float>& 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<float>& 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<float>& 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<float>& ogigin_val, OptAlgoResult& result) {
+  //
+
+  vector<float> super     = super_sampling(ogigin_val, ogigin_val.size(), 5);
+  vector<float> 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<float> 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<float> 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<float> 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
--- a/a8k_opt_algo.hpp
+++ b/a8k_opt_algo.hpp
@ -0,0 +1,68 @@
+#pragma once
+#include <fstream>
+#include <functional>
+#include <iostream>
+#include <list>
+#include <map>
+#include <memory>
+#include <set>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace a8k_opt_algo {
+using namespace std;
+
+/***********************************************************************************************************************
+ *                                                       预处理                                                        *
+ ***********************************************************************************************************************/
+
+class OptAlgoPreProcessResult {
+ public:
+  bool    scanAgain;
+  int32_t suggestScanGain;
+};
+
+void T_A8kOptAlgoPreProcess(vector<float>            ogigin_val,              //
+                            int32_t                  now_scan_gain,           //
+                            int32_t                  expectResultRangeStart,  //
+                            int32_t                  expectResultRangeEnd,    //
+                            OptAlgoPreProcessResult& result);
+void F_A8kOptAlgoPreProcess(vector<float>            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<float> displayData;  // 250           压缩后的数据
+
+  /**
+   * @brief 峰的信息
+   */
+  PeakInfo pin040;  // 峰040的信息
+  PeakInfo pin080;  // 峰080的信息
+  PeakInfo pin120;  // 峰120的信息
+  PeakInfo pin160;  // 峰160的信息
+  PeakInfo pin200;  // 峰200的信息
+};
+
+void A8kOptAlgoProcess(vector<float> ogigin_val, OptAlgoResult& result);
+
+}  // namespace a8k_opt_algo
--- a/build_pc.bat
+++ b/build_pc.bat
@ -0,0 +1 @@
+g++ a8k_opt_algo.cpp -fPIC -shared -o liba8k_opt_algo.so