YuanHongbin/LaserTracing
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
70758b1c06d6ff136106b15e0f317bd1f9d9a0d6
LaserTracing/source/src/Algorithm.c
T

784 lines
21 KiB
C
Raw Normal View History

初始版本
2026-04-23 14:02:03 +08:00
#include "bsp.h"
#include "Algorithm.h"
#include "UartDebug.h"
/*****************************************************************************************
* 函数名称: check_peaks_valleys_ratio
* 功能描述: 检查数据的峰值是否大于0，谷值是否小于0（忽略0值）
* 参 数: data, 数据
length, 数据长度
* 返 回 值: 结果返回峰值谷值检查结果结构体
*****************************************************************************************/
PeakValleyCheck check_peaks_valleys(int data[], int length) {
PeakValleyCheck result = {true, true, 0, 0};
// 如果数据长度小于3，无法形成有效的峰值/谷值
if (length < 3) {
return result;
}
int i = 0;
// 跳过开头的0值
while (i < length && data[i] == 0) {
i++;
}
// 遍历数据点
while (i < length) {
// 1. 寻找下一个非零点作为起点
int start = i;
while (i < length && data[i] == 0) {
i++;
}
if (i >= length) break;
// 2. 寻找当前非零段的结束点
int end = i;
while (end < length && data[end] != 0) {
end++;
}
end--; // 指向最后一个非零点
// 3. 在当前非零段中检测峰值和谷值
for (int j = i; j <= end; j++) {
// 跳过边界点
if (j == i || j == end) continue;
// 检查是否为峰值（大于左右相邻的非零点）
if (data[j] > data[j-1] && data[j] > data[j+1]) {
result.peak_count++;
if (data[j] <= 0) {
result.peaks_positive = false;
}
}
// 检查是否为谷值（小于左右相邻的非零点）
if (data[j] < data[j-1] && data[j] < data[j+1]) {
result.valley_count++;
if (data[j] >= 0) {
result.valleys_negative = false;
}
}
}
// 移动到下一段
i = end + 1;
}
return result;
}
/*****************************************************************************************
* 函数名称: analyze_trend
* 功能描述: 趋势分析函数
* 参 数: data, 数据
length, 数据长度
* 返 回 值: 结果返回趋势分析结构体
*****************************************************************************************/
TrendResult analyze_trend(int data[], int length) {
TrendResult result = {0};
#if 1
// 0. 检查有效数据长度
if (length < 2) {
DBG_LOG("Data deficient\r\n");
return result;
}
// 1. 计算线性回归斜率
double sum_x = 0, sum_y = 0, sum_xy = 0, sum_x2 = 0;
for (int i = 0; i < length; i++) {
double x = i; // 时间序列 (0,1,2,...)
double y = data[i];
sum_x += x;
sum_y += y;
sum_xy += x * y;
sum_x2 += x * x;
}
double numerator = length * sum_xy - sum_x * sum_y;
double denominator = length * sum_x2 - sum_x * sum_x;
// 处理分母为零的情况
if (fabs(denominator) > 1e-10) {
result.slope = numerator / denominator;
}
#endif
#if 0
// 2. 计算振荡特征（差分符号变化次数）
int sign_changes = 0;
int prev_sign = 0; // 0=未初始化, 1=正, -1=负
for (int i = 1; i < length; i++) {
int diff = data[i] - data[i-1];
int curr_sign = (diff > 0) ? 1 : (diff < 0) ? -1 : 0;
if (curr_sign != 0) {
if (prev_sign != 0 && curr_sign != prev_sign) {
sign_changes++;
}
prev_sign = curr_sign;
}
}
result.sign_changes = sign_changes;
#endif
#if 0
// 3. 动态计算阈值（基于数据长度）
int oscillation_threshold = (int)(0.4 * (length - 1)); // 40%的变化率
double slope_threshold = 0.05 * (length / 20.0); // 长度标准化
#endif
#if 0
// 4. 趋势判断
if (sign_changes >= oscillation_threshold) {
//DBG_LOG("Oscillating trend\r\n");//振荡趋势
result.trend_type = 0;
} else if (fabs(result.slope) < slope_threshold) {
//DBG_LOG("Smooth trend\r\n");//平稳趋势
result.trend_type = 1;
} else if (result.slope > 0) {
//DBG_LOG("Up trend\r\n");//上升趋势
result.trend_type = 2;
} else {
//DBG_LOG("Down trend\r\n");//下降趋势
result.trend_type = 3;
}
#endif
#if 0
// 5. 陡峭判断
double abs_slope = fabs(result.slope);
if (abs_slope > 0.8) {
//DBG_LOG("Steeply");//陡峭
result.sign_changes = 0;
} else if (abs_slope > 0.3) {
//DBG_LOG("Obvious");//明显
result.sign_changes = 1;
} else if (abs_slope > 0.1) {
//DBG_LOG("Mild");//温和
result.sign_changes = 2;
} else {
//DBG_LOG("gentle");//平缓
result.sign_changes = 3;
}
#endif
return result;
}
/*****************************************************************************************
* 函数名称: calculateAverage
* 功能描述: 计算平均值
* 参 数: arr, 数据
size, 数据长度
* 返 回 值: 结果返回 long long 防溢出
*****************************************************************************************/
float calculateAverage(int *arr, int size)
{
int sum = 0;
for (int i = 0; i < size; i++) {
sum += arr[i]; // 累加数组中的每个元素
}
return (float)sum / size; // 返回平均值
}
/*****************************************************************************************
* 函数名称: slope
* 功能描述: 计算斜率
* 参 数: x_data, x轴数据
y_data, y轴数据
* 返 回 值: 结果返回 long long 防溢出
*****************************************************************************************/
double slope(int32_t *x_data, int32_t *y_data, int n)
{
double sum_x = 0, sum_y = 0, sum_xx = 0, sum_xy = 0;
for (int i = 0; i < n; i++) {
sum_x += x_data[i];
sum_y += y_data[i];
sum_xx += pow(x_data[i] - calculateAverage(x_data, n), 2);
sum_xy += (x_data[i] - calculateAverage(x_data, n)) * (y_data[i] - calculateAverage(y_data, n));
}
return sum_xy / sum_xx;
}
/*****************************************************************************************
* 函数名称: power
* 功能描述: 快速幂算法
* 参 数: base, 滤波数据
exponent, 滤波数据长度
* 返 回 值: 结果返回 long long 防溢出
*****************************************************************************************/
long long power(int base, unsigned int exponent) {
long long result = 1;
while (exponent > 0) {
if (exponent % 2 == 1) {
result *= base; // 指数为奇数时累乘
}
base *= base; // 底数平方
exponent /= 2; // 指数折半
}
return result;
}
/*****************************************************************************************
* 函数名称: IntFilter_16t
* 功能描述: 16位数据中值滤波函数
* 参 数: Data, 滤波数据
Cnt, 滤波数据长度
FilterCnt, 滤除的数据长度，必须为2的倍数
* 返 回 值: 滤波后的16位数据
*****************************************************************************************/
int IntFilter_16t(int16_t *Data, uint8_t Cnt, uint8_t FilterCnt)
{
int32_t sum = 0;
int32_t temp;
if(Cnt < 2)
return *Data;
for(int j=0; j<Cnt-1; j++) {
for(int i=0; i<Cnt-j-1; i++) {
if(Data[i] > Data[i+1]) {
temp = Data[i];
Data[i] = Data[i+1];
Data[i+1] = temp;
}
}
}
for(int count = FilterCnt / 2; count < Cnt - FilterCnt / 2; count++)
sum += Data[count];
return (sum / (Cnt - FilterCnt));
}
/*****************************************************************************************
* 函数名称: IntFilter_32t
* 功能描述: 32位数据中值滤波函数
* 参 数: Data, 滤波数据
Cnt, 滤波数据长度
FilterCnt, 滤除的数据长度，必须为2的倍数
* 返 回 值: 滤波后的32位数据
*****************************************************************************************/
int IntFilter_32t(int32_t *Data, uint8_t Cnt, uint8_t FilterCnt)
{
int32_t sum = 0;
int32_t temp;
if(Cnt < 2)
return *Data;
for(int j=0; j<Cnt-1; j++) {
for(int i=0; i<Cnt-j-1; i++) {
if(Data[i] > Data[i+1]) {
temp = Data[i];
Data[i] = Data[i+1];
Data[i+1] = temp;
}
}
}
for(int count = FilterCnt / 2; count < Cnt - FilterCnt / 2; count++)
sum += Data[count];
return (sum / (Cnt - FilterCnt));
}
/*****************************************************************************************
* 函数名称: IntFilter_u32t
* 功能描述: 32位无符号位数据中值滤波函数
* 参 数: Data, 滤波数据
Cnt, 滤波数据长度
FilterCnt, 滤除的数据长度，必须为2的倍数
* 返 回 值: 滤波后的32位数据
*****************************************************************************************/
uint32_t IntFilter_u32t(uint32_t *Data, uint8_t Cnt, uint8_t FilterCnt)
{
uint32_t sum = 0;
uint32_t temp;
if(Cnt < 2)
return *Data;
for(int j=0; j<Cnt-1; j++) {
for(int i=0; i<Cnt-j-1; i++) {
if(Data[i] > Data[i+1]) {
temp = Data[i];
Data[i] = Data[i+1];
Data[i+1] = temp;
}
}
}
for(int count = FilterCnt / 2; count < Cnt - FilterCnt / 2; count++)
sum += Data[count];
return (sum / (Cnt - FilterCnt));
}
/*****************************************************************************************
* 函数名称: IntFilter_Float
* 功能描述: 32位浮点数数据中值滤波函数
* 参 数: Data, 滤波数据
Cnt, 滤波数据长度
FilterCnt, 滤除的数据长度，必须为2的倍数
* 返 回 值: 滤波后的32位数据
*****************************************************************************************/
float IntFilter_Float(float *Data, uint8_t Cnt, uint8_t FilterCnt)
{
float sum = 0;
float temp;
if(Cnt < 2)
return *Data;
for(int j=0; j<Cnt-1; j++) {
for(int i=0; i<Cnt-j-1; i++) {
if(Data[i] > Data[i+1]) {
temp = Data[i];
Data[i] = Data[i+1];
Data[i+1] = temp;
}
}
}
for(int count = FilterCnt / 2; count < Cnt - FilterCnt / 2; count++)
sum += Data[count];
return (sum / (Cnt - FilterCnt));
}
/*****************************************************************************************
* 函数名称: AverageFilter_u32t
* 功能描述: 32位数据均值滤波函数,去除了最大值和最小值
* 参 数: Data, 滤波数据
Cnt, 滤波数据长度
* 返 回 值: 滤波后的32位数据
*****************************************************************************************/
uint32_t AverageFilter_u32t(uint32_t *Data, uint8_t Cnt)
{
uint32_t sum = 0;
uint32_t temp;
uint32_t max = Data[0];
uint32_t min = Data[0];
if(Cnt == 0)
return 0;
if(Cnt == 1)
return Data[0];
if(Cnt == 2)
{
sum = Data[0] + Data[1];
return sum / 2;
}
for (uint8_t i = 0; i < Cnt; i++)//找出最大值
{
if (Data[i] > max)
{
max = Data[i];
}
}
for (uint8_t i = 0; i < Cnt; i++)//找出最小值
{
if (Data[i] < min)
{
min = Data[i];
}
}
for(int i = 0; i < Cnt; i++){//求和
sum += Data[i];
}
return (sum - max - min) / (Cnt - 2);
}
/*****************************************************************************************
* 函数名称: AverageFilter_32t
* 功能描述: 32位数据均值滤波函数,去除了最大值和最小值
* 参 数: Data, 滤波数据
Cnt, 滤波数据长度
* 返 回 值: 滤波后的32位数据
*****************************************************************************************/
int AverageFilter_32t(int32_t *Data, uint8_t Cnt)
{
int32_t sum = 0;
int32_t max = Data[0];
int32_t min = Data[0];
if(Cnt == 0)
return 0;
if(Cnt == 1)
return Data[0];
if(Cnt == 2)
{
sum = Data[0] + Data[1];
return sum / 2;
}
for (uint8_t i = 0; i < Cnt; i++)//找出最大值
{
if (Data[i] > max)
{
max = Data[i];
}
}
for (uint8_t i = 0; i < Cnt; i++)//找出最小值
{
if (Data[i] < min)
{
min = Data[i];
}
}
for(int i = 0; i < Cnt; i++){//求和
sum += Data[i];
}
return (sum - max - min) / (Cnt - 2);
}
/*****************************************************************************************
* 函数名称: Fitting_Polynomial
* 功能描述: 根据数组AD[], Actual[]列出的一组数据，用最小二乘法求它的拟合曲线,默认3阶
近似解析表达式为y = a3*x^3 + a2*x^2 + a1*x + a0;
* 参 数: AD, AD芯片采样值
Actual, 实际标校值
Cnt, 拟合数据个数
* 返 回 值: 无
*****************************************************************************************/
void Fitting_Polynomial(double *AD, double *Actual, uint8_t Cnt)
{
static const uint8_t rank_ = 3;//拟合阶数3
double atemp[2 * (rank_ + 1)], b[rank_ + 1], a[rank_ + 1][rank_ + 1];
int i, j, k;
for(i = 0; i < Cnt; i++){
atemp[1] += AD[i];
atemp[2] += pow(AD[i], 2);
atemp[3] += pow(AD[i], 3);
atemp[4] += pow(AD[i], 4);
atemp[5] += pow(AD[i], 5);
atemp[6] += pow(AD[i], 6);
b[0] += Actual[i];
b[1] += AD[i] * Actual[i];
b[2] += pow(AD[i], 2) * Actual[i];
b[3] += pow(AD[i], 3) * Actual[i];
}
atemp[0] = Cnt;
for(i = 0; i < rank_ + 1; i++){ //构建线性方程组系数矩阵，b[]不变
k = i;
for(j = 0; j < rank_ + 1; j++) a[i][j] = atemp[k++];
}
//以下为高斯列主元消去法解线性方程组
for(k = 0; k < rank_ + 1 - 1; k++){ //n - 1列
int column = k;
double mainelement = a[k][k];
for(i = k; i < rank_ + 1; i++) //找主元素
if(fabs(a[i][k]) > mainelement){
mainelement = fabs(a[i][k]);
column = i;
}
for(j = k; j < rank_ + 1; j++){ //交换两行
double atemp = a[k][j];
a[k][j] = a[column][j];
a[column][j] = atemp;
}
double btemp = b[k];
b[k] = b[column];
b[column] = btemp;
for(i = k + 1; i < rank_ + 1; i++){ //消元过程
double Mik = a[i][k] / a[k][k];
for(j = k; j < rank_ + 1; j++) a[i][j] -= Mik * a[k][j];
b[i] -= Mik * b[k];
}
}
b[rank_ + 1 - 1] /= a[rank_ + 1 - 1][rank_ + 1 - 1]; //回代过程
for(i = rank_ + 1 - 2; i >= 0; i--){
double sum = 0;
for(j = i + 1; j < rank_ + 1; j++) sum += a[i][j] * b[j];
b[i] = (b[i] - sum) / a[i][i];
}//高斯列主元消去法结束
DBG_LOG("P(x) = %.16fx^3%+.16fx^2%+.16fx%+.16f\r\n", b[3], b[2], b[1], b[0]);
// App.Para.Cali.FitCoef[0] = b[0];
// App.Para.Cali.FitCoef[1] = b[1];
// App.Para.Cali.FitCoef[2] = b[2];
// App.Para.Cali.FitCoef[3] = b[3];
}
/*****************************************************************************************
* 函数名称: get_K
* 功能描述: 斜率计算
* 参 数: count，数据个数 数组行(列)的个数 数组的行列数目相等
dataCol_X[count]，数据的列数据
dataRow_Y[count]，数据的行数据
* 返 回 值: k 斜率
*****************************************************************************************/
float get_K(uint8_t count , int32_t *dataCol_X, int32_t *dataRow_Y)
{
float k = 0;//斜率
float aveCol_X = 0;//列的平均值x
float aveRow_Y = 0;//行的平均值y
int32_t sum_XY = 0;//行列的总和xy
int32_t sumRow_Y = 0;//行的总和y
int32_t sumCol_X = 0;//列的总和x
int32_t sumCol_X2 = 0;//列的总和x^2
for(uint16_t i = 0 ; i < count ; i++)
{
sumCol_X += dataCol_X[i];//求列x的总和
sumRow_Y += dataRow_Y[i];//求行y的总和
sumCol_X2 += dataCol_X[i] * dataCol_X[i];//求x^2的总和
sum_XY += (dataCol_X[i] * dataRow_Y[i]);//求xy的总和
}
aveCol_X = 1.0 * sumCol_X / count;//求平均值
aveRow_Y = 1.0 * sumRow_Y / count;
k = (sum_XY - aveCol_X * aveRow_Y * count) / //根据公式求斜率
(sumCol_X2 - aveCol_X * aveCol_X * count);
return k;
}
/*****************************************************************************************
* 函数名称: TrendAnalyse
* 功能描述: 判断数组中的值的总体趋势
* 参 数: Data,
Cnt,
VPT,
* 返 回 值: true或false
*****************************************************************************************/
int8_t TrendAnalyse(int32_t *Data, uint8_t Cnt, int32_t VPT)
{
int8_t zero = 0,plus = 0, minus = 0, Trend = 0xEE;
if(Cnt < 2)
return Trend;
for(uint8_t i = 1; i < Cnt; i++)
{
if((Data[i] - Data[i - 1]) <= VPT && ((Data[i] - Data[i - 1]) >= (-VPT)))
{
zero++;
}
else if((Data[i] - Data[i - 1]) > VPT)
{
plus++;
}
else if((Data[i] - Data[i - 1]) < VPT)
{
minus++;
}
}
if(zero > (Cnt-(Cnt / 5)))
Trend = 0;//振荡趋势
else if(plus > (Cnt-(Cnt/ 5)))
Trend = 1;//上升趋势
else if(minus > (Cnt-(Cnt/ 5)))
Trend = -1;//下降趋势
return Trend;//总趋势
}
/*****************************************************************************************
* 函数名称: Waveform_Up
* 功能描述: 找出一段波形的波峰值
dCnt, 波形数据长度
pCnt, 要查找波峰数
vlue, 返回的波峰值
* 返 回 值: true或false
*****************************************************************************************/
void Waveform_Up(int32_t *Data, uint8_t dCnt, uint8_t pCnt, int32_t *vlue)
{
uint8_t peak[pCnt];
for(uint8_t i = 0, j = 0; i < dCnt; i++)//找出峰值地址
{
if(Data[i] < Data[i + 1] && Data[i + 1] > Data[i + 2])
{
peak[j++] = i;
}
if(j == pCnt)
break;
}
for(uint8_t i = 0; i < pCnt; i++)
{
vlue[i] = Data[peak[i]];
}
}
/*****************************************************************************************
* 函数名称: Waveform_Down
* 功能描述: 找出一段波形的波谷值
dCnt, 波形数据长度
pCnt, 要查找波谷数
vlue, 返回的波谷值
* 返 回 值: true或false
*****************************************************************************************/
void Waveform_Down(int32_t *Data, uint8_t dCnt, uint8_t pCnt, int32_t *vlue)
{
uint8_t peak[pCnt];
for(uint8_t i = 0, j = 0; i < dCnt; i++)//找出峰值地址
{
if(Data[i] > Data[i + 1] && Data[i + 1] < Data[i + 2])
{
peak[j++] = i;
}
if(j == pCnt)
break;
}
for(uint8_t i = 0; i < pCnt; i++)
{
vlue[i] = Data[peak[i]];
}
}
/*****************************************************************************************
* 函数名称: count_most_greater
* 功能描述: 判断数组中的值是否大部分大于 target
* 参 数: arr,
size,
target,
* 返 回 值: true或false
*****************************************************************************************/
bool count_most_greater(int32_t *arr, uint8_t size, int32_t target)
{
uint8_t cnt = 0;
for (int i = 0; i < size; i++) {
if (arr[i] < target)
cnt++;
if(cnt > (size - (size / 5)))
return false;
}
return true;
}
/*****************************************************************************************
* 函数名称: count_greater
* 功能描述: 判断数组中的值是否全部大于 target
* 参 数: arr,
size,
target,
* 返 回 值: true或false
*****************************************************************************************/
bool count_greater(int32_t *arr, uint8_t size, int32_t target)
{
for (int i = 0; i < size; i++) {
if (arr[i] < target)
return false;
}
return true;
}
/*****************************************************************************************
* 函数名称: count_smaller
* 功能描述: 判断数组中的值是否全部小于 target
* 参 数: arr,
size,
target,
* 返 回 值: true或false
*****************************************************************************************/
bool count_smaller(int32_t *arr, uint8_t size, int32_t target)
{
for (int i = 0; i < size; i++) {
if (arr[i] > target)
return false;
}
return true;
}
/*****************************************************************************************
* 函数名称: CRC_Modbus
* 功能描述: CRC16计算函数
* 参 数: wBase, 多项式
Para，校验数据入口
Data, 校验数据长度入口
* 返 回 值: crc16校验值
*****************************************************************************************/
uint16_t CRC_Modbus(uint16_t wBase, uint8_t *para, uint16_t length)
{
uint16_t crc = 0xffff;
uint16_t index,i;
for(index = 0 ; index < length;index++) {
crc ^= para[index];
for(i = 0; i < 8; i++) {
if(crc & 1) {
crc >>= 1;
crc ^= wBase;
}
else
crc >>= 1;
}
}
return crc;
}
/*****************************************************************************************
* 函数名称: CRC_Sum
* 功能描述: 从第二个字节开始，求和取反
* 参 数: _pbuff，校验数据入口
_cmdLen，校验数据长度入口
* 返 回 值: cmd_sum，校验值(一个字节)
*****************************************************************************************/
uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen)
{
uint8_t cmd_sum=0;
uint16_t i;
for(i=1;i<_cmdLen;i++)//从1开始，跳过第一个字节
{
cmd_sum += _pbuff[i];
}
cmd_sum = (~cmd_sum);
return cmd_sum;
}
1、Algorithm文件的isAllZero函数改为可改对比参数的is_all_byte
2026-05-11 18:23:00 +08:00
/**
* @brief 检查缓冲区是否全部等于指定字节值
* @param data : 待检查的数据指针
* @param len : 数据长度（字节数）
* @param val : 期望的字节值 (0x00 ~ 0xFF)
* @return true : 全部等于 val
* false: 存在不等于 val 的字节
*/
bool is_all_byte(const uint8_t *data, size_t len, uint8_t val)
初始版本
2026-04-23 14:02:03 +08:00
{
1、Algorithm文件的isAllZero函数改为可改对比参数的is_all_byte
2026-05-11 18:23:00 +08:00
for (size_t i = 0; i < len; i++) {
if (data[i] != val) {
return false;
初始版本
2026-04-23 14:02:03 +08:00
}
}
1、Algorithm文件的isAllZero函数改为可改对比参数的is_all_byte
2026-05-11 18:23:00 +08:00
return true;
初始版本
2026-04-23 14:02:03 +08:00
}
/*****************************************************************************************
* 函数名称: HexToAscii
* 功能描述: 16进制转ASCII码
* 参 数: HexData，16进制数组
ASCData，ASCII码
sLen，数据长度
* 返 回 值: true或false
*****************************************************************************************/
void HexToAscii(uint8_t *HexData, char *ASCData, uint8_t sLen)
{
uint8_t temp;
for(int i = 0; i < sLen; i++) {
temp = (HexData[i] >> 4) & 0x0f;
if(temp < 10)
temp += '0';
else
temp = (temp - 10) + 'A';
ASCData[i * 2] = temp;
temp = HexData[i] & 0x0f;
if(temp < 10)
temp += '0';
else
temp = (temp - 10) + 'A';
ASCData[i * 2 + 1] = temp;
}
ASCData[sLen * 2] = '\r';
ASCData[sLen * 2 + 1] = '\n';
ASCData[sLen * 2 + 2] = 0;
}
Reference in New Issue Copy Permalink