You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
456 lines
14 KiB
456 lines
14 KiB
#include "a8k_opt_algo.hpp"
|
|
|
|
#include <stdarg.h>
|
|
#ifdef BUILD_IN_QT
|
|
void zos_log(const char* fmt, ...);
|
|
|
|
#define ZLOGI(fmt, ...) zos_log("INFO " fmt "\n", ##__VA_ARGS__);
|
|
#define ZLOGD(fmt, ...) zos_log("DEBU " fmt "\n", ##__VA_ARGS__);
|
|
#define ZLOGE(fmt, ...) zos_log("ERRO " fmt "\n", ##__VA_ARGS__);
|
|
#define ZLOGW(fmt, ...) zos_log("WARN " fmt "\n", ##__VA_ARGS__);
|
|
|
|
#else
|
|
#define ZLOGI(fmt, ...) ;
|
|
#define ZLOGD(fmt, ...) ;
|
|
#define ZLOGE(fmt, ...) ;
|
|
#define ZLOGW(fmt, ...) ;
|
|
|
|
#endif
|
|
|
|
namespace a8k_opt_algo {
|
|
using namespace std;
|
|
|
|
typedef enum {
|
|
ktopt,
|
|
kfopt,
|
|
} opt_type_t;
|
|
|
|
static void algo_assert(bool condition, const char* msg) {
|
|
if (!condition) {
|
|
ZLOGE("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 findPeakStartTurnPoint(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 search_end = suggest_search_end;
|
|
int32_t peakTurnPos = search_start;
|
|
float maxdiff2 = m_cxt.diffX2[search_start];
|
|
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;
|
|
}
|
|
|
|
int32_t findPeakEndTurnPoint(vector<float>& data, int32_t search_start, int32_t suggest_search_end) {
|
|
int32_t search_end = suggest_search_end;
|
|
int32_t peakTurnPos = search_start;
|
|
float maxdiff2 = m_cxt.diffX2[search_start];
|
|
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
|
|
ZLOGI("find peak in [%d %d]", search_start, search_end);
|
|
retpeak.find_peak = false;
|
|
retpeak.area = 0;
|
|
retpeak.peak_pos = 0;
|
|
retpeak.peak_start_pos = 0;
|
|
retpeak.peak_end_pos = 0;
|
|
|
|
float max = 0;
|
|
int peakpos = 0;
|
|
float midpos = search_start + (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) {
|
|
ZLOGI("invalid peak:%f, max < m_cxt.agvline:%f", max, m_cxt.agvline);
|
|
retpeak.find_peak = false;
|
|
return;
|
|
} else if (peakpos > midpos + 15) {
|
|
ZLOGI("invalid peak:%d, peakpos > midpos + 15:%d", peakpos, midpos + 15);
|
|
retpeak.find_peak = false;
|
|
return;
|
|
} else if (peakpos < midpos - 15) {
|
|
ZLOGI("invalid peak:%d, peakpos < midpos - 15:%d", peakpos, midpos - 15);
|
|
retpeak.find_peak = false;
|
|
return;
|
|
}
|
|
|
|
// find_peak_start
|
|
// 从pos向前找20个点，从低于均值线的坐标开始找，找到diff2的最大值
|
|
retpeak.peak_pos = peakpos;
|
|
retpeak.peak_start_pos = findPeakStartTurnPoint(data, peakpos - 20, peakpos) - 4; //-4 是经验数值
|
|
retpeak.peak_end_pos = findPeakEndTurnPoint(data, peakpos, peakpos + 20) + 4; //+4 是经验数值
|
|
retpeak.area = computePeakArea(data, retpeak.peak_start_pos, retpeak.peak_end_pos);
|
|
retpeak.find_peak = true;
|
|
|
|
// 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(super, 24);
|
|
ZLOGI("subsample size:%d", subsample.size());
|
|
|
|
result.input = ogigin_val;
|
|
m_cxt.raw = subsample;
|
|
m_cxt.avg = subsample;
|
|
m_cxt.diff = least_square_method_differentiate(m_cxt.avg, 5); // 最小二乘法求曲线斜率集合
|
|
m_cxt.diffX2 = least_square_method_differentiate(m_cxt.diff, 5); // 最小二乘法求曲线二次斜率集合
|
|
m_cxt.agvline = find_avg_line(subsample);
|
|
result.displayData = m_cxt.avg;
|
|
|
|
// 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);
|
|
}
|
|
}
|
|
|
|
ecode_t A8kOptAlgoProcess(vector<float> ogigin_val, OptAlgoResult& result) { //
|
|
if (ogigin_val.size() != 1200) {
|
|
return kOptErr_pointNumError;
|
|
}
|
|
a8k_opt_algo_process(ogigin_val, result);
|
|
return kOptErr_suc;
|
|
}
|
|
|
|
void A8kOptAlgoGetProcessContext(PorcessContext& context) { context = m_cxt; }
|
|
|
|
/***********************************************************************************************************************
|
|
* 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;
|
|
}
|
|
|
|
ecode_t T_A8kOptAlgoPreProcess(vector<float> ogigin_val, int32_t now_scan_gain, int32_t expectResultRangeStart, int32_t expectResultRangeEnd, OptAlgoPreProcessResult& result) {
|
|
if (ogigin_val.size() != 1200) {
|
|
return kOptErr_pointNumError;
|
|
}
|
|
|
|
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 kOptErr_suc;
|
|
}
|
|
|
|
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;
|
|
return kOptErr_suc;
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* 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;
|
|
}
|
|
ecode_t F_A8kOptAlgoPreProcess(vector<float> ogigin_val, int32_t now_scan_gain, int32_t expectResultRangeStart, int32_t expectResultRangeEnd, OptAlgoPreProcessResult& result) {
|
|
if (ogigin_val.size() != 1200) {
|
|
return kOptErr_pointNumError;
|
|
}
|
|
|
|
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 kOptErr_suc;
|
|
}
|
|
|
|
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;
|
|
return kOptErr_suc;
|
|
}
|
|
|
|
} // namespace a8k_opt_algo
|