228 lines
6.7 KiB
C
228 lines
6.7 KiB
C
|
||
#include "find_peaks.h"
|
||
#include "MyTool.h"
|
||
|
||
void compute_dynamic_threshold_optimized(const float *data, float *thresholds)
|
||
{
|
||
float buffer[WINDOW_SIZE]; // 循环缓冲区
|
||
int buf_idx = 0; // 缓冲区当前写入位置
|
||
float window_sum = 0.0f;
|
||
|
||
// 第一阶段：初始化缓冲区
|
||
for (int i = 0; i < WINDOW_SIZE; i++)
|
||
{
|
||
float energy = data[i] * data[i];
|
||
buffer[buf_idx] = energy;
|
||
window_sum += energy;
|
||
buf_idx = (buf_idx + 1) % WINDOW_SIZE;
|
||
}
|
||
|
||
// 第二阶段：计算首个有效阈值
|
||
float first_threshold = DYNAMIC_THRESH_RATIO * sqrtf(window_sum / WINDOW_SIZE);
|
||
|
||
// 填充前WINDOW_SIZE个阈值（与原算法行为一致）
|
||
for (int i = 0; i < WINDOW_SIZE; i++)
|
||
{
|
||
thresholds[i] = first_threshold;
|
||
}
|
||
|
||
// 第三阶段：滑动窗口处理后续数据
|
||
for (int i = WINDOW_SIZE; i < DATA_LENGTH; i++)
|
||
{
|
||
float new_energy = data[i] * data[i];
|
||
float old_energy = buffer[buf_idx]; // 获取最早的能量值
|
||
|
||
window_sum += new_energy - old_energy; // 更新滑动总和
|
||
buffer[buf_idx] = new_energy; // 更新缓冲区
|
||
buf_idx = (buf_idx + 1) % WINDOW_SIZE; // 移动循环指针
|
||
|
||
// 计算动态阈值
|
||
thresholds[i] = DYNAMIC_THRESH_RATIO * sqrtf(window_sum / WINDOW_SIZE);
|
||
}
|
||
}
|
||
|
||
// 滑动平均预处理
|
||
void moving_average(float *data)
|
||
{
|
||
float buffer[MOVING_AVG_WINDOW] = {0.0f};
|
||
float sum = 0.0f;
|
||
|
||
// 初始化窗口
|
||
for (int i = 0; i < MOVING_AVG_WINDOW; i++)
|
||
{
|
||
buffer[i] = data[i];
|
||
sum += data[i];
|
||
}
|
||
|
||
// 滑动处理
|
||
for (int i = MOVING_AVG_WINDOW; i < DATA_LENGTH; i++)
|
||
{
|
||
sum = sum - buffer[i % MOVING_AVG_WINDOW] + data[i];
|
||
buffer[i % MOVING_AVG_WINDOW] = data[i];
|
||
data[i - MOVING_AVG_WINDOW / 2] = sum / MOVING_AVG_WINDOW;
|
||
}
|
||
}
|
||
|
||
PeakResult find_peaks(const float *input)
|
||
{
|
||
PeakResult result_struct = {
|
||
.peaks = {0}, // 初始化peaks数组全为0
|
||
.thresholds = {0.0f}, // 初始化thresholds数组全为0.0f
|
||
.count = 0}; // 初始化count为0
|
||
|
||
// memset(&result_struct,0,sizeof(result_struct));
|
||
float data[DATA_LENGTH] = {0.0f};
|
||
int i = 0, j = 0;
|
||
|
||
// 拷贝并预处理数据
|
||
memcpy(data, input, sizeof(float) * DATA_LENGTH);
|
||
moving_average(data);
|
||
|
||
// 第一阶段：候选波峰检测
|
||
int candidates[MAX_CANDIDATES] = {0};
|
||
int candidate_count = 0;
|
||
float prev_diff = 0.0f;
|
||
|
||
for (i = 1; i < DATA_LENGTH; i++)
|
||
{
|
||
float diff = data[i] - data[i - 1];
|
||
if (prev_diff > 0 && diff < 0)
|
||
{ // 斜率由正转负
|
||
candidates[candidate_count++] = i - 1;
|
||
}
|
||
prev_diff = diff;
|
||
}
|
||
|
||
// 第二阶段：动态阈值计算
|
||
compute_dynamic_threshold_optimized(data, result_struct.thresholds); // 动态阈值
|
||
|
||
// 第三阶段：峰值筛选
|
||
int last_peak = -MIN_PEAK_DISTANCE;
|
||
for (j = 0; j < candidate_count; j++)
|
||
{
|
||
const int idx = candidates[j];
|
||
if (data[idx] > result_struct.thresholds[idx] &&
|
||
(idx - last_peak) >= MIN_PEAK_DISTANCE)
|
||
{
|
||
result_struct.peaks[result_struct.count++] = idx;
|
||
last_peak = idx;
|
||
}
|
||
if (result_struct.count >= sizeof(result_struct.peaks) / sizeof(int))
|
||
break;
|
||
}
|
||
|
||
return result_struct;
|
||
}
|
||
|
||
int calculate_heart_rate(const PeakResult *result)
|
||
{
|
||
const int sample_rate = SAMPLE_RATE; // 采样率50Hz
|
||
float avg_interval = 0.0f;
|
||
|
||
if (result->count < 2)
|
||
{
|
||
return -1; // 波峰不足无法计算心率
|
||
}
|
||
|
||
// 计算相邻波峰的平均间隔（网页1和网页7的结合）
|
||
int total_intervals = 0;
|
||
for (int i = 1; i < result->count; i++)
|
||
{
|
||
int interval = result->peaks[i] - result->peaks[i - 1]; // 间隔的点数
|
||
|
||
if (interval >= 30 && interval <= 116)
|
||
{
|
||
total_intervals += interval;
|
||
}
|
||
else
|
||
{
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
// 计算平均间隔时间（秒）
|
||
avg_interval = total_intervals / (float)(result->count - 1) / sample_rate;
|
||
int hp_output = 0;
|
||
hp_output = (int)round(60.0f / avg_interval); // 60秒 / 平均间隔秒数
|
||
return hp_output;
|
||
}
|
||
|
||
int time_domain_heart_rate(float *hp_data)
|
||
{
|
||
PeakResult time_result = find_peaks(hp_data);
|
||
|
||
int hr = calculate_heart_rate(&time_result);
|
||
if (hr < 30 || hr > 150)
|
||
{ // 生理范围验证
|
||
return -2; // 异常心率代码
|
||
}
|
||
return hr;
|
||
}
|
||
|
||
//
|
||
float compute_target_acf(const float *data, int lag, float mean, float total_energy)
|
||
{
|
||
const int N = DATA_LENGTH - lag;
|
||
if (N <= 0 || total_energy < 1e-6f)
|
||
return 0.0f;
|
||
|
||
float sum_product = 0.0f;
|
||
for (int i = 0; i < N; i++)
|
||
{
|
||
float x = data[i] - mean;
|
||
float y = data[i + lag] - mean;
|
||
sum_product += x * y;
|
||
}
|
||
return sum_product / total_energy;
|
||
}
|
||
// 快速选择心率
|
||
int fast_select_hr(const float candidates[4], const float *data, float *fft_energy_ratios)
|
||
{
|
||
float32_t std = 0.0f;
|
||
float32_t avg = 0.0f;
|
||
avg = MyToolAvgValue((float32_t *)data, DATA_LENGTH);
|
||
std = MyToolStdValue((float32_t *)data, DATA_LENGTH);
|
||
// 预处理计算均值和总能量
|
||
float parsed_data[DATA_LENGTH];
|
||
// memset(parsed_data, data, sizeof(float) * DATA_LENGTH);
|
||
float mean = 0.0f, total_energy = 0.0f;
|
||
for (int i = 0; i < DATA_LENGTH; i++)
|
||
{
|
||
parsed_data[i] = (data[i] - avg) / std;
|
||
mean += parsed_data[i];
|
||
total_energy += parsed_data[i] * parsed_data[i];
|
||
}
|
||
mean /= DATA_LENGTH;
|
||
total_energy -= DATA_LENGTH * mean * mean; // 修正总能量
|
||
// 遍历候选心率
|
||
float max_score = -1.0f;
|
||
int selected_idx = -1;
|
||
float temp = 0.0f;
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
const float hr = (i < 2) ? candidates[i] / 2 : candidates[i];
|
||
// 转换候选心率为lag值
|
||
int target_lag = (int)(50 * 60.0f / hr + 0.5f);
|
||
if (target_lag <= 0 || target_lag >= DATA_LENGTH / 2)
|
||
continue;
|
||
// 局部窗口ACF峰值检测
|
||
float acf_peak = -1.0f;
|
||
int search_radius = 1;
|
||
for (int offset = 0; offset <= search_radius; offset++)
|
||
{
|
||
int current_lag = target_lag + offset;
|
||
if (current_lag < 1 || current_lag >= DATA_LENGTH - 1)
|
||
continue;
|
||
float acf_val = compute_target_acf(parsed_data, current_lag, mean, total_energy);
|
||
acf_peak = fmax(acf_peak, acf_val);
|
||
}
|
||
// 综合评分（网页6的加权策略）
|
||
float score = 0.7f * acf_peak + 0.3f * fft_energy_ratios[i];
|
||
if (score > max_score)
|
||
{
|
||
max_score = score;
|
||
selected_idx = i;
|
||
}
|
||
}
|
||
return selected_idx;
|
||
} |