511 lines
13 KiB
C
511 lines
13 KiB
C
/*
|
||
* @Description:
|
||
* @Version: 1.0
|
||
* @Author: lzc
|
||
* @Date: 2023-07-21 15:42:16
|
||
* @LastEditors: lzc
|
||
* @LastEditTime: 2024-12-26 17:07:56
|
||
*/
|
||
|
||
#include "MyTool.h"
|
||
|
||
/**
|
||
* @function: MyToolFindMaxIndex
|
||
* @brief 获取最大值索引
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {uint32_t} Index 返回最大值的地址
|
||
* @author: lzc
|
||
*/
|
||
uint32_t MyToolFindMaxIndex(float32_t *Input, uint32_t Length)
|
||
{
|
||
uint32_t Index = 0;
|
||
float32_t MaxValue = 0.0f;
|
||
arm_max_f32(Input, Length, &MaxValue, &Index);
|
||
return Index;
|
||
}
|
||
/**
|
||
* @function: MyToolFindMaxValue
|
||
* @brief 获取最大值
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {float32_t} MaxValue 返回最大值
|
||
* @author: lzc
|
||
*/
|
||
float32_t MyToolFindMaxValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
uint32_t Index = 0;
|
||
float32_t MaxValue = 0.0f;
|
||
arm_max_f32(Input, Length, &MaxValue, &Index);
|
||
return MaxValue;
|
||
}
|
||
/**
|
||
* @function: MyToolFindMinValue
|
||
* @brief 获取最小值
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {float32_t} MaxValue 返回最小值
|
||
* @author: lzc
|
||
*/
|
||
float32_t MyToolFindMinValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
uint32_t Index = 0;
|
||
float32_t MinValue = 0.0f;
|
||
arm_min_f32(Input, Length, &MinValue, &Index);
|
||
return MinValue;
|
||
}
|
||
/**
|
||
* @function: MyToolStdValue
|
||
* @brief 标准差
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float32_t MyToolStdValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
float32_t Value = 0.0f;
|
||
arm_std_f32(Input, Length, &Value);
|
||
return Value;
|
||
}
|
||
|
||
/**
|
||
* @function: MyToolAvgValue
|
||
* @brief 平均值
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float32_t MyToolAvgValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
float32_t Value = 0.0f;
|
||
arm_mean_f32(Input, Length, &Value);
|
||
return Value;
|
||
}
|
||
|
||
/**
|
||
* @function: MyToolSumValue
|
||
* @brief 求和
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float32_t MyToolSumValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
float32_t Temp = 0.0f;
|
||
float32_t SumValue = 0.0f;
|
||
for (uint32_t i = 0; i < Length; i++)
|
||
{
|
||
arm_add_f32(&Input[i], &Temp, &SumValue, 1);
|
||
Temp = SumValue;
|
||
}
|
||
return SumValue;
|
||
}
|
||
// TODO: 此处进行排序
|
||
/*
|
||
降序 ————选择排序
|
||
*/
|
||
char sort(float a[], int n)
|
||
{
|
||
float temp;
|
||
int i, j, index;
|
||
for (i = 0; i < n - 1; i++)
|
||
{
|
||
index = i;
|
||
for (j = i + 1; j < n; j++)
|
||
{ // 寻找最小值
|
||
if (a[index] < a[j]) // a[i]若不是最小
|
||
index = j; // 记录最小值下标
|
||
}
|
||
if (index != i) // 最小值和第i个记录进行互换
|
||
{
|
||
temp = a[i];
|
||
a[i] = a[index];
|
||
a[index] = temp; // a[i]的值赋给被交换的位置
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
/**
|
||
* @function: MyToolSumValue
|
||
* @brief 排序
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void MyToolSortValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
float32_t MyOutput[Length];
|
||
for (int i = 0; i < Length; i++)
|
||
MyOutput[i] = Input[i];
|
||
sort(MyOutput, Length);
|
||
for (int i = 0; i < Length; i++)
|
||
{
|
||
Input[i] = MyOutput[i];
|
||
// printf("MyOutput %d :%f \r\n", i, MyOutput[i]);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @function: MyToolSumValue
|
||
* @brief 排序
|
||
* @param {float32_t} *Input 浮点数组地址
|
||
* @param {uint32_t} Length 长度
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float32_t MyToolMidValue(float32_t *Input, uint32_t Length)
|
||
{
|
||
int MidPos = 0;
|
||
float32_t MyOutput[Length];
|
||
for (int i = 0; i < Length; i++)
|
||
MyOutput[i] = Input[i];
|
||
sort(MyOutput, Length);
|
||
MidPos = Length % 2;
|
||
if (MidPos == 1) // 奇数
|
||
{
|
||
MidPos = (Length / 2);
|
||
return MyOutput[MidPos];
|
||
}
|
||
else if (MidPos == 0) // 偶数
|
||
{
|
||
MidPos = (Length / 2);
|
||
return (MyOutput[MidPos] + MyOutput[MidPos + 1]) / 2;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @function: SetRandom
|
||
* @brief: None
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void MySetRandom(uint16_t value)
|
||
{
|
||
uint8_t temp[12];
|
||
uint32_t temp0, temp1, temp2;
|
||
temp0 = *(__IO uint32_t *)(ID_ADDR); // 产品唯一身份标识寄存器（96位）
|
||
temp1 = *(__IO uint32_t *)(ID_ADDR + 4);
|
||
temp2 = *(__IO uint32_t *)(ID_ADDR + 8);
|
||
temp[0] = (uint8_t)(temp0 & 0x000000FF);
|
||
temp[1] = (uint8_t)((temp0 & 0x0000FF00) >> 8);
|
||
temp[2] = (uint8_t)((temp0 & 0x00FF0000) >> 16);
|
||
temp[3] = (uint8_t)((temp0 & 0xFF000000) >> 24);
|
||
temp[4] = (uint8_t)(temp1 & 0x000000FF);
|
||
temp[5] = (uint8_t)((temp1 & 0x0000FF00) >> 8);
|
||
temp[6] = (uint8_t)((temp1 & 0x00FF0000) >> 16);
|
||
temp[7] = (uint8_t)((temp1 & 0xFF000000) >> 24);
|
||
temp[8] = (uint8_t)(temp2 & 0x000000FF);
|
||
temp[9] = (uint8_t)((temp2 & 0x0000FF00) >> 8);
|
||
temp[10] = (uint8_t)((temp2 & 0x00FF0000) >> 16);
|
||
temp[11] = (uint8_t)((temp2 & 0xFF000000) >> 24);
|
||
srand((unsigned)temp + value);
|
||
}
|
||
/**
|
||
* @function:
|
||
* @brief: None
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
static uint8_t MyBreathRate;
|
||
static uint8_t MyHeartBeatRate;
|
||
extern float KalmarFilter(float inData);
|
||
int MyRadomHeart(void)
|
||
{
|
||
MyHeartBeatRate = (int)(((float)(rand() % (6) + 60)) + 2);
|
||
return (MyHeartBeatRate);
|
||
}
|
||
|
||
/**
|
||
* @function:
|
||
* @brief: None
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
int MyRadomBreath(void)
|
||
{
|
||
MyBreathRate = (int)(((float)(rand() % (6) + 10)) + 2);
|
||
return (MyBreathRate);
|
||
}
|
||
/**
|
||
* @function:move_update
|
||
* @brief: None
|
||
* @param {float} arr
|
||
* @param {int} len
|
||
* @param {float} new_f
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void MyToolMoveUpdate(float arr[], int len, float new_f)
|
||
{
|
||
int i;
|
||
for (i = 0; i < len - 1; i++)
|
||
{
|
||
arr[i] = arr[i + 1];
|
||
}
|
||
arr[len - 1] = new_f;
|
||
}
|
||
|
||
// 比较函数，用于降序排序
|
||
int compare(const void *a, const void *b)
|
||
{
|
||
float diff = *(float *)b - *(float *)a;
|
||
return (diff > 0) ? 1 : (diff < 0) ? -1
|
||
: 0;
|
||
}
|
||
|
||
// 寻找峰值元素的函数
|
||
int findPeaks(float *nums, int numsSize, int **peakIndices, float **peakValues)
|
||
{
|
||
int count = 0; // 峰值的数量
|
||
*peakIndices = (int *)malloc(numsSize * sizeof(int)); // 分配峰值索引数组
|
||
*peakValues = (float *)malloc(numsSize * sizeof(float)); // 分配峰值数组
|
||
|
||
// 检查中间的元素
|
||
for (int i = 1; i < numsSize - 1; i++)
|
||
{
|
||
if (nums[i] > nums[i - 1] && nums[i] > nums[i + 1])
|
||
{
|
||
(*peakIndices)[count] = i;
|
||
(*peakValues)[count] = nums[i];
|
||
count++;
|
||
}
|
||
}
|
||
return count; // 返回峰值的数量
|
||
}
|
||
|
||
int peakProcess(float *nums, int numsSize, int *max_peak_index, float *max_peak_value)
|
||
{
|
||
int *peakIndices = NULL; // 存储峰值索引的数组
|
||
float *peakValues = NULL; // 存储峰值的数组
|
||
int peakCount = 0; // 峰值的数量
|
||
int value_peak_count = 0;
|
||
|
||
// 寻找所有峰值元素
|
||
peakCount = findPeaks(nums, numsSize, &peakIndices, &peakValues);
|
||
|
||
// 如果有峰值，按照峰值的大小降序排列索引
|
||
if (peakCount > 0)
|
||
{
|
||
qsort(peakValues, peakCount, sizeof(float), compare);
|
||
// 根据排序后的峰值数组，重新排列峰值索引
|
||
for (int i = 0; i < peakCount; i++)
|
||
{
|
||
if (value_peak_count >= 2)
|
||
break;
|
||
|
||
for (int j = 0; j < peakCount; j++)
|
||
{
|
||
if (value_peak_count >= 2)
|
||
break;
|
||
if (peakValues[i] == nums[peakIndices[j]])
|
||
{
|
||
// log_i("Index: %d Value :%f\n", peakIndices[j], peakValues[i]);
|
||
if (max_peak_index[0] == 0)
|
||
{
|
||
max_peak_index[0] = peakIndices[j];
|
||
max_peak_value[0] = peakValues[i];
|
||
value_peak_count = value_peak_count + 1;
|
||
}
|
||
if ((max_peak_index[0] > 0) && (max_peak_index[1] == 0) && (abs(peakIndices[j] - max_peak_index[0]) > 3))
|
||
{
|
||
max_peak_index[1] = peakIndices[j];
|
||
max_peak_value[1] = peakValues[i];
|
||
value_peak_count = value_peak_count + 1;
|
||
break;
|
||
}
|
||
|
||
peakIndices[j] = -1; // 标记已打印的索引
|
||
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
// 释放内存
|
||
free(peakIndices);
|
||
free(peakValues);
|
||
return value_peak_count;
|
||
}
|
||
|
||
// 查找最接近元素的函数
|
||
float find_closest_element(const float arr[], int length, int target)
|
||
{
|
||
if (target == 0)
|
||
return arr[1];
|
||
|
||
float half_arr0 = arr[0] / 2.0f;
|
||
|
||
// 初始化为第一个元素
|
||
float closest = half_arr0;
|
||
float min_diff = fabs(half_arr0 - target); // 初始最小差值
|
||
for (int i = 1; i < length; i++)
|
||
{
|
||
float val = (i < 1) ? (arr[i] / 2.0f) : arr[i]; // 仅前两个元素除以2
|
||
float current_diff = fabs(val - target);
|
||
// 如果找到更小的差值，更新最接近元素
|
||
if (current_diff < min_diff)
|
||
{
|
||
min_diff = current_diff;
|
||
closest = val;
|
||
}
|
||
}
|
||
return closest;
|
||
}
|
||
int MyToolSelectHp(float *hpList, float *ratioList, float *formerHps, uint16_t time_result)
|
||
{
|
||
int res = 0;
|
||
|
||
float highMax = hpList[0];
|
||
float highSecond = hpList[1];
|
||
float loMax = hpList[2];
|
||
float loSecond = hpList[3];
|
||
|
||
float highFilterRatio = ratioList[0];
|
||
float lowFilterRatio = ratioList[1];
|
||
|
||
float noZeroSum = 0.0f;
|
||
int nearest_value = 0;
|
||
int noZeroAvg = 0;
|
||
int noZeroCount = 0;
|
||
|
||
for (int i = 0; i < 15; i++)
|
||
{
|
||
if (formerHps[i] != 0.0f)
|
||
{
|
||
noZeroCount++;
|
||
noZeroSum += formerHps[i];
|
||
}
|
||
}
|
||
|
||
if (noZeroCount > 0)
|
||
noZeroAvg = (int)round(noZeroSum / noZeroCount);
|
||
|
||
if (fabs(loMax * 2.0f - highMax) <= 5 && loMax >= 45 && loMax <= 120) // 倍频明显的直接返回
|
||
{
|
||
res = (int)round(loMax);
|
||
return res;
|
||
}
|
||
|
||
if (fabs(loSecond * 2.0f - highMax) <= 4 && loSecond >= 60 && loSecond <= 105) // 倍频明显的直接返回
|
||
{
|
||
res = (int)round(loSecond);
|
||
return res;
|
||
}
|
||
|
||
if (fabs(loMax * 2.0f - highSecond) < 5 && loMax >= 55 && loMax <= 105 && highSecond <= 190) // 倍频与本频很接近的直接返回
|
||
{
|
||
res = (int)round(loMax);
|
||
return res;
|
||
}
|
||
|
||
if (fabs(loMax - highMax) < 5 && loMax >= 55 && loMax <= 120 && highMax <= 120 && noZeroCount > 10) // 倍频峰和本峰差不多
|
||
{
|
||
if (fabs(loMax - noZeroAvg) < 4)
|
||
{
|
||
res = (int)round(loMax);
|
||
return res;
|
||
}
|
||
else if (fabs(highMax / 2.0f - noZeroAvg) < 4 && highMax > 120)
|
||
{
|
||
res = (int)round(highMax / 2.0f);
|
||
return res;
|
||
}
|
||
}
|
||
|
||
if (fabs(loSecond - highMax) < 4 && loSecond >= 55 && loSecond <= 105 && highMax <= 110 && noZeroCount > 10)
|
||
{
|
||
if (fabs(loSecond - noZeroAvg) < 4)
|
||
{
|
||
res = (int)round(loSecond);
|
||
return res;
|
||
}
|
||
else if (fabs(loSecond / 2.0f - noZeroAvg) < 4 && loSecond > 120)
|
||
{
|
||
res = (int)round(loSecond / 2.0f);
|
||
return res;
|
||
}
|
||
}
|
||
|
||
if (noZeroCount > 10 && fabs(loMax - noZeroAvg) < 4 && loMax >= 50 && loMax <= 105) // 与前15s的均值相近 直接输出
|
||
{
|
||
res = (int)round(loMax);
|
||
return res;
|
||
}
|
||
|
||
if (noZeroCount > 10 && fabs(noZeroAvg * 2.0f - highMax) < 4 && highMax > 100) // 与前15s的均值相近 直接输出
|
||
{
|
||
res = (int)round(highMax / 2.0f);
|
||
return res;
|
||
}
|
||
|
||
if (noZeroCount > 10 && fabs(noZeroAvg - loSecond) < 3 && loSecond >= 50 && loSecond <= 105 && noZeroAvg >= 60) // 与前15s的均值相近 直接输出
|
||
{
|
||
res = (int)round(loSecond);
|
||
return res;
|
||
}
|
||
|
||
if (lowFilterRatio <= 0.5f && lowFilterRatio <= highFilterRatio && loMax >= 45 && loMax <= 100)
|
||
{
|
||
res = (int)round(loMax);
|
||
return res;
|
||
}
|
||
|
||
if (highFilterRatio <= 0.5f && highFilterRatio <= lowFilterRatio && highMax >= 120 && highMax <= 200)
|
||
{
|
||
res = (int)round(highMax / 2.0f);
|
||
return res;
|
||
}
|
||
|
||
if (highFilterRatio <= 0.5f && highFilterRatio <= lowFilterRatio && highMax >= 45 && highMax <= 95)
|
||
{
|
||
res = (int)round(highMax);
|
||
}
|
||
|
||
else if (fabs(loSecond - highMax) < 5 && highMax >= 120 && highMax <= 210)
|
||
{
|
||
res = (int)round(highMax / 2.0f);
|
||
}
|
||
|
||
else if (fabs(loSecond - highMax) < 5 && loSecond >= 60 && loSecond <= 90)
|
||
{
|
||
res = (int)round(loSecond);
|
||
}
|
||
|
||
else if (loMax >= 55 && loMax <= 95 && highFilterRatio > lowFilterRatio)
|
||
{
|
||
res = (int)round(loMax);
|
||
}
|
||
|
||
else if (highMax > 110)
|
||
{
|
||
res = (int)round(highMax / 2.0f);
|
||
}
|
||
|
||
else if (time_result >= 60 && time_result <= 110 && noZeroCount > 10 && fabs(time_result - noZeroAvg) < 3)
|
||
{
|
||
res = time_result;
|
||
}
|
||
|
||
else
|
||
{
|
||
res = (int)round(loMax);
|
||
}
|
||
|
||
if (res == 0)
|
||
{
|
||
if (time_result >= 55 && time_result <= 110)
|
||
{
|
||
res = time_result;
|
||
}
|
||
else
|
||
{
|
||
res = (int)round(loMax);
|
||
}
|
||
}
|
||
return res;
|
||
} |