1430 lines
44 KiB
C
1430 lines
44 KiB
C
/*
|
||
* @Description:
|
||
* @Version: 1.0
|
||
* @Author: lzc
|
||
* @Date: 2023-07-24 09:56:14
|
||
* @LastEditors: lzc
|
||
* @LastEditTime: 2024-12-26 17:01:22
|
||
*/
|
||
#include <main.h>
|
||
#include <math.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include "opm.h"
|
||
#include "cwt.h"
|
||
#include "find_peaks.h"
|
||
|
||
#include "MyTool.h"
|
||
#include "MyAlgorithm.h"
|
||
#include "MyBandFitter.h"
|
||
|
||
#define step 0.009775171065493637f
|
||
int outPut_HeartBeat = 0;
|
||
|
||
void heartDataPostProcessing(void);
|
||
|
||
float amp[HEART_ARRAY_LENGTH][1] = {0.0f}; // 矩阵输出
|
||
float ampSum[HEART_ARRAY_LENGTH] = {0.0f}; // 矩阵输出
|
||
float ampSumRr[BREATH_ARRAY_LENGTH] = {0.0f}; // 矩阵输出
|
||
|
||
float f_BPM_hr[5] = {0, 0, 0, 0, 0};
|
||
float f_BPM_hr2[4] = {0, 0, 0, 0};
|
||
float f_BPM_br[5] = {0, 0, 0, 0, 0};
|
||
float f_BPM_br2[4] = {0, 0, 0, 0};
|
||
// 数据后处理时计数数据上升与下降次数
|
||
int count_up_hr = 0, count_down_hr = 0, count_up_hr_m = 0, count_down_hr_m = 0, count_out_range_hr = 0;
|
||
// 数据后处理时计数数据上升与下降次数
|
||
int count_up_br = 0, count_down_br = 0, count_up_br_m = 0, count_down_br_m = 0, count_out_range_br = 0;
|
||
|
||
void breathDataPostProcessing(void);
|
||
/**
|
||
* @function:
|
||
* @brief: None
|
||
* @param {float32_t} *Input
|
||
* @param {uint8_t} Output_HeartBeat
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float HeartDataArray[LENGTH_SAMPLES_HEART] = {0};
|
||
void cwt_getHeartBeat(float32_t *Input)
|
||
{
|
||
float out_hr = 0.0f;
|
||
int n = LENGTH_SAMPLES_HEART; // 每次计算数据长度为400（8s）
|
||
int i_max_hr = 0; // 幅值最大值所在行（对应心率频率）
|
||
float max_sum = 0.0f; // 求最大幅值
|
||
float sum_amp_hr[HEART_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
|
||
memset(amp, 0, sizeof(float) * HEART_ARRAY_LENGTH * 1);
|
||
memset(ampSum, 0, sizeof(float) * HEART_ARRAY_LENGTH);
|
||
memcpy(HeartDataArray, Input, sizeof(float) * n);
|
||
printf("Start Heart!!!\r\n");
|
||
cwt_hr((float *)HeartDataArray, n, amp); // 心率小波转换
|
||
// for (int i = 0; i < HEART_ARRAY_LENGTH; i++) // 对每行幅值数据求和
|
||
// sum_amp_hr[i] = MyToolSumValue(amp[i], n);
|
||
// memcpy(sum_amp_hr, ampSum, sizeof(float) * HEART_ARRAY_LENGTH);
|
||
// i_max_hr = MyToolFindMaxIndex(sum_amp_hr, HEART_ARRAY_LENGTH);
|
||
// max_sum = sum_amp_hr[i_max_hr];
|
||
heartDataPostProcessing();
|
||
printf("!!!!! HeartBeatRate Date %d !!!!!!\r\n", HeartBeatRate);
|
||
printf("End Heart!!!\r\n");
|
||
}
|
||
|
||
/**
|
||
* @function: cwt_getBreathRate
|
||
* @brief 获取呼吸
|
||
* @param {float32_t} *Input
|
||
* @param {uint8_t} Output_BreathRate
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float BreathDataArray[LENGTH_SAMPLES_BREATH] = {0};
|
||
void cwt_getBreathRate(float32_t *Input)
|
||
{
|
||
float out_rr = 0.0f;
|
||
int n = LENGTH_SAMPLES_BREATH; // 每次计算数据长度为400（8s）
|
||
int i_max_br = 0; // 幅值最大值所在行（对应心率频率）
|
||
float max_sum = 0.0f; // 求最大幅值
|
||
float sum_amp_br[BREATH_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
|
||
memset(amp, 0, sizeof(float) * BREATH_ARRAY_LENGTH * 1);
|
||
memset(ampSumRr, 0, sizeof(float) * BREATH_ARRAY_LENGTH);
|
||
memcpy(BreathDataArray, Input, sizeof(float) * n);
|
||
printf("Start!!!\r\n");
|
||
cwt_rr((float *)BreathDataArray, n, amp); // 心率小波转换
|
||
// for (int i = 0; i < BREATH_ARRAY_LENGTH; i++) // 对每行幅值数据求和
|
||
// sum_amp_br[i] = MyToolSumValue(amp[i], n);
|
||
// memcpy(sum_amp_br, ampSumRr, sizeof(float) * BREATH_ARRAY_LENGTH);
|
||
// i_max_br = MyToolFindMaxIndex(ampSumRr, BREATH_ARRAY_LENGTH);
|
||
// max_sum = ampSumRr[i_max_br];
|
||
breathDataPostProcessing();
|
||
printf("!!!!! BreathRate Date %d !!!!!!\r\n", BreathRate);
|
||
printf("End!!!\r\n");
|
||
}
|
||
|
||
/**
|
||
* @function: freq_calculated
|
||
* @brief: None
|
||
* @param {int} i_max
|
||
* @param {int} index_rr_hr
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float freq_calculated(int i_max, int index_rr_hr)
|
||
{
|
||
float fre = 0.0f;
|
||
if (index_rr_hr == 0) // 心率频率索引
|
||
{
|
||
fre = (2.294921 - (i_max + 13) * 0.024414);
|
||
}
|
||
else
|
||
{ // 呼吸率索引
|
||
fre = (2.294921 - (i_max + 74) * 0.024414);
|
||
}
|
||
return fre;
|
||
}
|
||
|
||
/**
|
||
* @function: amp_reset
|
||
* @brief: None
|
||
* @param {int} n
|
||
* @param {float} arr
|
||
* @param {int} k
|
||
* @param {int} index_rr_hr
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void amp_reset(int n, float arr[][n], int k, int index_rr_hr)
|
||
{
|
||
int row = HEART_ARRAY_LENGTH;
|
||
if (index_rr_hr != 0)
|
||
{
|
||
row = 12;
|
||
}
|
||
for (int i = 0; i < row; i++)
|
||
{
|
||
for (int j = (50 * k); j < (50 * (k + 1)); j++)
|
||
{
|
||
arr[i][j] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @function:move_update
|
||
* @brief: None
|
||
* @param {float} arr
|
||
* @param {int} len
|
||
* @param {float} new_f
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void move_update(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;
|
||
}
|
||
|
||
/**
|
||
* @function:min_compare
|
||
* @brief: None
|
||
* @param {float} a
|
||
* @param {float} b
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float min_compare(float a, float b)
|
||
{
|
||
float c;
|
||
if (a < b)
|
||
{
|
||
c = a;
|
||
}
|
||
else
|
||
{
|
||
c = b;
|
||
}
|
||
return c;
|
||
}
|
||
/**
|
||
* @function:max_compare
|
||
* @brief: None
|
||
* @param {float} a
|
||
* @param {float} b
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float max_compare(float a, float b)
|
||
{
|
||
float c;
|
||
if (a > b)
|
||
{
|
||
c = a;
|
||
}
|
||
else
|
||
{
|
||
c = b;
|
||
}
|
||
return c;
|
||
}
|
||
|
||
/**
|
||
* @function: med_calculate_2
|
||
* @brief: None
|
||
* @param {float} arr
|
||
* @param {int} len
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float med_calculate_2(float arr[], int len)
|
||
{
|
||
int i, j, k; // 定义循环因子，嵌套双层循环
|
||
float med;
|
||
float arr2[len];
|
||
for (i = 0; i < len; i++)
|
||
{
|
||
arr2[i] = arr[i];
|
||
}
|
||
for (j = 0; j < len - 1; j++) // 设置循环后界
|
||
{
|
||
for (k = 0; k < len - j - 1; k++) // 不断向后进行比较
|
||
{
|
||
if (arr2[k] < arr2[k + 1]) // 比较相邻的元素
|
||
{
|
||
float temp = arr2[k]; // 三杯水交换法
|
||
arr2[k] = arr2[k + 1];
|
||
arr2[k + 1] = temp;
|
||
}
|
||
}
|
||
}
|
||
if (arr2[1] == 0 && arr2[0] > 0)
|
||
{
|
||
med = arr2[0];
|
||
}
|
||
else if (arr2[2] == 0 && arr2[1] > 0)
|
||
{
|
||
med = (arr2[0] + arr2[1]) / 2;
|
||
}
|
||
else if (arr2[3] == 0 && arr2[2] > 0)
|
||
{
|
||
med = arr2[1];
|
||
}
|
||
else if (arr2[4] == 0 && arr2[3] > 0)
|
||
{
|
||
med = (arr2[2] + arr2[1]) / 2;
|
||
}
|
||
else if (len % 2 == 0)
|
||
{ // 奇偶取中位数的方法不一样
|
||
med = (arr2[(len / 2) - 1] + arr2[(len / 2)]) / 2;
|
||
}
|
||
else
|
||
{
|
||
med = arr2[(len - 1) / 2];
|
||
}
|
||
return med;
|
||
}
|
||
/**
|
||
* @function:heartDataPostProcessing
|
||
* @brief 心率数据后处理
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void heartDataPostProcessing(void)
|
||
{
|
||
int n = 400;
|
||
int i_max_hr = 0; // 幅值最大值所在行（对应心率频率）
|
||
float sum_amp_hr[HEART_ARRAY_LENGTH]; // 心率幅值求和
|
||
// float sum_amp_hr_sec[8][HEART_ARRAY_LENGTH] = {0};
|
||
// int nfNum[8] = {0};
|
||
int nf = 0;
|
||
i_max_hr = MyToolFindMaxIndex(ampSum, HEART_ARRAY_LENGTH);
|
||
nf = i_max_hr;
|
||
|
||
move_update(f_BPM_hr, 5, round(60 * freq_calculated(nf, 0)));
|
||
if (f_BPM_hr[4] >= 40 && f_BPM_hr[4] <= 110)
|
||
f_BPM_hr[4] = round(((f_BPM_hr[4] - 40) / 20) + 2.5f + f_BPM_hr[4]);
|
||
else
|
||
f_BPM_hr[4] = round(8 + f_BPM_hr[4]); // note: 增加对应的偏置
|
||
#if LINE_COEFF_EN
|
||
if (HeartBeatRate >= 83 && HeartBeatRate <= 110)
|
||
HeartBeatRate = round((HeartBeatRate - 83) * 0.23 + 3 + HeartBeatRate);
|
||
#else
|
||
;
|
||
#endif
|
||
//========================================================================数据后处理
|
||
// if (f_BPM_hr[0] > 0 && f_BPM_hr2[0] == 0) // 保证最开始的前2个数据可靠性
|
||
// {
|
||
// if (fabs(f_BPM_hr[0] - MyToolMidValue(f_BPM_hr, 5)) > 6)
|
||
// {
|
||
// f_BPM_hr[0] = MyToolMidValue(f_BPM_hr, 5);
|
||
// }
|
||
// if (fabs(f_BPM_hr[1] - MyToolMidValue(f_BPM_hr, 5)) > 6)
|
||
// {
|
||
// f_BPM_hr[1] = MyToolMidValue(f_BPM_hr, 5);
|
||
// }
|
||
// }
|
||
for (int i = 0; i < 4; i++) // 把前4个数据复制出来供后处理分析
|
||
{
|
||
f_BPM_hr2[i] = f_BPM_hr[i];
|
||
}
|
||
if (f_BPM_hr[0] > 0) // 当数据满5个之后开始后处理
|
||
{
|
||
if (f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) >= (12)) // 比前4个中位数大12
|
||
{
|
||
count_up_hr += 1;
|
||
if (count_up_hr + count_up_hr_m >= 3)
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (4 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 4 : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
count_down_hr = 0;
|
||
count_down_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
else if (f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) <= (-8)) // 比前4个中位数小8
|
||
{
|
||
count_down_hr += 1;
|
||
if (count_down_hr + count_down_hr_m >= 3)
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-3) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-3) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
count_up_hr = 0;
|
||
count_up_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
else if (f_BPM_hr[4] - f_BPM_hr[3] > 0)
|
||
{
|
||
if (f_BPM_hr[4] - f_BPM_hr[3] >= 4)
|
||
{
|
||
count_up_hr_m += 1;
|
||
if (count_down_hr + count_down_hr_m >= 3)
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (3 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 3 : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
else if (count_up_hr + count_up_hr_m >= 3)
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + 3;
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + 1;
|
||
}
|
||
}
|
||
count_down_hr = 0;
|
||
count_down_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
else if (f_BPM_hr[4] - f_BPM_hr[3] < 0)
|
||
{
|
||
if (f_BPM_hr[4] - f_BPM_hr[3] <= (-4))
|
||
{
|
||
count_down_hr_m += 1;
|
||
if (count_up_hr + count_up_hr_m >= 3)
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
else if (count_down_hr + count_down_hr_m >= 3)
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (-2);
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (-1);
|
||
}
|
||
}
|
||
count_up_hr = 0;
|
||
count_up_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
if (f_BPM_hr[4] <= 55)
|
||
{
|
||
f_BPM_hr[4] = 55;
|
||
}
|
||
else if (f_BPM_hr[4] > 120)
|
||
{
|
||
f_BPM_hr[4] = 120;
|
||
}
|
||
HeartBeatRate = f_BPM_hr[4];
|
||
|
||
// printf("------------------- Freq %.6f\r\n", freq_calculated(nf, 0));
|
||
}
|
||
else
|
||
{
|
||
int Count = 0;
|
||
float sum, avg = 0.0f;
|
||
for (char i = 0; i < 5; i++)
|
||
{
|
||
// printf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
|
||
if ((f_BPM_hr[i]) != 0)
|
||
{
|
||
// printf("Rand Data :%d\r\n", (rand() % (5)));
|
||
sum += (f_BPM_hr[i]);
|
||
Count++;
|
||
}
|
||
}
|
||
avg = sum / Count;
|
||
HeartBeatRate = (uint8_t)((round(avg)));
|
||
if (HeartBeatRate <= 55)
|
||
{
|
||
HeartBeatRate = 55;
|
||
}
|
||
else if (HeartBeatRate > 120)
|
||
{
|
||
HeartBeatRate = 120;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @function: cwt_getHeartBeat_immediately
|
||
* @brief: None
|
||
* @param {float32_t} *Input
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void cwt_getHeartBeat_immediately(float32_t *Input)
|
||
{
|
||
int n = 400; // 每次计算数据长度为400（8s）
|
||
int i_max_hr = 0; // 幅值最大值所在行（对应心率频率）
|
||
float max_sum = 0.0f; // 求最大幅值
|
||
float sum_amp_hr[HEART_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
|
||
memset(amp, 0, sizeof(float) * 45 * 400);
|
||
memcpy(HeartDataArray, Input, sizeof(float) * 200);
|
||
memcpy(HeartDataArray + 200, Input, sizeof(float) * 200);
|
||
// 拼数组
|
||
// printf("Start!!!\r\n");
|
||
MySetRandom(AIN_1310);
|
||
cwt_hr((float *)HeartDataArray, n, amp); // 心率小波转换
|
||
for (int i = 0; i < HEART_ARRAY_LENGTH; i++) // 对每行幅值数据求和
|
||
sum_amp_hr[i] = MyToolSumValue(amp[i], n);
|
||
i_max_hr = MyToolFindMaxIndex(sum_amp_hr, HEART_ARRAY_LENGTH);
|
||
max_sum = sum_amp_hr[i_max_hr];
|
||
heartDataPostProcessing();
|
||
// 如果数据错误采用随机值
|
||
if (HeartBeatRate > 75 || HeartBeatRate < 60)
|
||
MyRadomHeart();
|
||
// printf("!!!!! HeartBeatRate Date %d !!!!!!\r\n", HeartBeatRate);
|
||
memset(HeartDataArray, 0, sizeof(HeartDataArray));
|
||
memset(f_BPM_hr2, 0, sizeof(f_BPM_hr2));
|
||
memset(f_BPM_hr, 0, sizeof(f_BPM_hr));
|
||
// printf("End!!!\r\n");
|
||
}
|
||
|
||
/**
|
||
* @function: cwt_getHeartBeat_Test
|
||
* @brief: 测试通过滤波数据出心率
|
||
* @param {float32_t} *Input
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void cwt_getHeartBeat_Test(void)
|
||
{
|
||
int n = 400; // 每次计算数据长度为400（8s）
|
||
int size = 0;
|
||
int i_max_hr = 0; // 幅值最大值所在行（对应心率频率）
|
||
float max_sum = 0.0f; // 求最大幅值
|
||
float sum_amp_hr[HEART_ARRAY_LENGTH] = {0.0f}; // 心率幅值求和
|
||
memset(amp, 0, sizeof(float) * 45 * 400);
|
||
// printf("Start!!!\r\n");
|
||
size = sizeof(cwtTestBuffer) / sizeof(float);
|
||
// printf("size is %d\r\n", size);
|
||
for (char i = 0; i < 2 * (size / n); i++)
|
||
{
|
||
memcpy(HeartDataArray, cwtTestBuffer + (200 * i), sizeof(float) * 400);
|
||
cwt_hr((float *)HeartDataArray, n, amp); // 心率小波转换
|
||
for (int i = 0; i < HEART_ARRAY_LENGTH; i++) // 对每行幅值数据求和
|
||
sum_amp_hr[i] = MyToolSumValue(amp[i], n);
|
||
i_max_hr = MyToolFindMaxIndex(sum_amp_hr, HEART_ARRAY_LENGTH);
|
||
max_sum = sum_amp_hr[i_max_hr];
|
||
heartDataPostProcessing();
|
||
// printf("!!!!! HeartBeatRate Date %d !!!!!!\r\n", HeartBeatRate);
|
||
}
|
||
memset(HeartDataArray, 0, sizeof(HeartDataArray));
|
||
memset(f_BPM_hr2, 0, sizeof(f_BPM_hr2));
|
||
memset(f_BPM_hr, 0, sizeof(f_BPM_hr));
|
||
// printf("End!!!\r\n");
|
||
}
|
||
|
||
/**
|
||
* @function:breathDataPostProcessing
|
||
* @brief 呼吸数据后处理
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void breathDataPostProcessing(void)
|
||
{
|
||
int n = 400;
|
||
int i_max_br = 0; // 幅值最大值所在行（对应呼吸频率）
|
||
float sum_amp_br[12]; // 呼吸幅值求和
|
||
// float sum_amp_br_sec[8][12] = {0};
|
||
int nfNum[8] = {0};
|
||
int nf = 0;
|
||
i_max_br = MyToolFindMaxIndex(ampSumRr, 12);
|
||
nf = i_max_br;
|
||
move_update(f_BPM_br, 5, round(60 * freq_calculated(nf, 1)));
|
||
//========================================================================数据后处理
|
||
if (f_BPM_br[0] > 0 && f_BPM_br2[0] == 0) // 保证最开始的前2个数据可靠性
|
||
{
|
||
if (fabs(f_BPM_br[0] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
|
||
{
|
||
f_BPM_br[0] = MyToolMidValue(f_BPM_br, 5);
|
||
}
|
||
if (fabs(f_BPM_br[1] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
|
||
{
|
||
f_BPM_br[1] = MyToolMidValue(f_BPM_br, 5);
|
||
}
|
||
}
|
||
for (int i = 0; i < 4; i++) // 把前4个数据复制出来供后处理分析
|
||
{
|
||
f_BPM_br2[i] = f_BPM_br[i];
|
||
}
|
||
if (f_BPM_br[0] > 0) // 当数据满5个之后开始后处理
|
||
{
|
||
if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (10) ||
|
||
f_BPM_br[4] - f_BPM_br[3] >= 8) // 比前4个中位数大12
|
||
{
|
||
count_out_range_br += 1;
|
||
if (count_out_range_br > 1)
|
||
{
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
}
|
||
if (count_out_range_br >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(3, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3];
|
||
}
|
||
}
|
||
else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= (-10) ||
|
||
f_BPM_br[4] - f_BPM_br[3] <= (-8)) // 比前4个中位数小8
|
||
{
|
||
count_out_range_br += 1;
|
||
if (count_out_range_br > 1)
|
||
{
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
}
|
||
if (count_out_range_br >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-3), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3];
|
||
}
|
||
}
|
||
else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (6) ||
|
||
f_BPM_br[4] - f_BPM_br[3] >= 3) // 较大值判断
|
||
{
|
||
// if (i_nodata_br == 0)
|
||
// {
|
||
// count_up_br += 1;
|
||
// }
|
||
if (count_up_br + count_up_br_m >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(1.5f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else if (count_up_br + count_up_br_m >= 2)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(1, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(0.2f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_out_range_br = 0;
|
||
}
|
||
else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= ((-6) || f_BPM_br[4] - f_BPM_br[3] <= (-3))) // 较小值判断
|
||
{
|
||
// if (i_nodata_br == 0)
|
||
// {
|
||
// count_down_br += 1;
|
||
// }
|
||
if (count_down_br + count_down_br_m >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-1.5f), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else if (count_down_br + count_down_br_m >= 2)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-0.2f), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
count_out_range_br = 0;
|
||
}
|
||
else if (f_BPM_br[4] - f_BPM_br[3] > 0)
|
||
{
|
||
// if (i_nodata_br == 0)
|
||
// {
|
||
// count_up_br_m += 1;
|
||
// }
|
||
if (count_up_br + count_up_br_m >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(2, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else if (count_up_br + count_up_br_m >= 2)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(1, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(0.3f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_out_range_br = 0;
|
||
}
|
||
else if (f_BPM_br[4] - f_BPM_br[3] < 0)
|
||
{
|
||
// if (i_nodata_br == 0)
|
||
// {
|
||
// count_down_br_m += 1;
|
||
// }
|
||
if (count_down_br + count_down_br_m >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-2), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else if (count_down_br + count_down_br_m >= 2)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-0.3f), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
count_out_range_br = 0;
|
||
}
|
||
|
||
if (f_BPM_br[4] < 10)
|
||
{
|
||
f_BPM_br[4] = 10;
|
||
}
|
||
else if (f_BPM_br[4] > 25)
|
||
{
|
||
f_BPM_br[4] = 25;
|
||
}
|
||
|
||
BreathRate = round(f_BPM_br[4]);
|
||
// note: 增加对应的偏置
|
||
#if LINE_COEFF_EN
|
||
if (BreathRate >= 13 && BreathRate <= 30)
|
||
BreathRate = round((BreathRate - 13) * 0.2 + BreathRate);
|
||
#else
|
||
;
|
||
#endif
|
||
// printf("----------------------------Frq: %.6f\r\n", freq_calculated(nf, 1));
|
||
// printf("br-i_max: %d %d %.6f\r\n", i_max_br, BreathRate, freq_calculated(nf, 1));
|
||
}
|
||
else
|
||
{
|
||
int Count = 0;
|
||
float sum, avg = 0.0f;
|
||
for (char i = 0; i < 5; i++)
|
||
{
|
||
// printf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
|
||
if ((f_BPM_br[i]) != 0)
|
||
{
|
||
// printf("Rand Data :%d\r\n", (rand() % (5)));
|
||
sum += (f_BPM_br[i]);
|
||
Count++;
|
||
}
|
||
}
|
||
avg = sum / Count;
|
||
BreathRate = (uint8_t)(round(avg));
|
||
if (BreathRate < 10)
|
||
{
|
||
BreathRate = 10;
|
||
}
|
||
else if (BreathRate > 25)
|
||
{
|
||
BreathRate = 25;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @function:cwt_getBreath_Test
|
||
* @brief 测试通过滤波数据出呼吸
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void cwt_getBreath_Test(void)
|
||
{
|
||
int n = 400; // 每次计算数据长度为400（8s）
|
||
int size = 0;
|
||
int i_max_br = 0; // 幅值最大值所在行（对应心率频率）
|
||
float max_sum = 0.0f; // 求最大幅值
|
||
float sum_amp_br[12] = {0.0f}; // 心率幅值求和
|
||
memset(amp, 0, sizeof(float) * 45 * 400);
|
||
// 拼数组
|
||
printf("Start!!!\r\n");
|
||
size = sizeof(cwtBreathBuffer) / sizeof(float);
|
||
printf("size is %d\r\n", size);
|
||
for (char i = 0; i < 2 * (size / n); i++)
|
||
{
|
||
memcpy(BreathDataArray, cwtBreathBuffer + (200 * i), sizeof(float) * 400);
|
||
cwt_rr((float *)BreathDataArray, n, amp); // 心率小波转换
|
||
for (int i = 0; i < 12; i++) // 对每行幅值数据求和
|
||
sum_amp_br[i] = MyToolSumValue(amp[i], n);
|
||
i_max_br = MyToolFindMaxIndex(sum_amp_br, 12);
|
||
max_sum = sum_amp_br[i_max_br];
|
||
breathDataPostProcessing();
|
||
printf("!!!!! BreathRate Date %d !!!!!!\r\n", BreathRate);
|
||
}
|
||
memset(BreathDataArray, 0, sizeof(BreathDataArray));
|
||
memset(f_BPM_br2, 0, sizeof(f_BPM_br2));
|
||
memset(f_BPM_br, 0, sizeof(f_BPM_br));
|
||
printf("End!!!\r\n");
|
||
}
|
||
|
||
#include "MyBandFitter.h"
|
||
|
||
/**
|
||
* @function: fft_Output
|
||
* @brief: None
|
||
* @param {float32_t} *Input
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
#define FFT_POINT 2048
|
||
uint32_t FFT_Size = FFT_POINT;
|
||
float32_t FFT_InPut[FFT_POINT] = {0};
|
||
float32_t FFT_OutPut[FFT_POINT] = {0};
|
||
float32_t FFT_OutPut_Real[FFT_POINT] = {0};
|
||
|
||
float stash_Ratio[ARRAY_NUMBER] = {0};
|
||
uint16_t fft_HeartBeatCurrentFilter = 0;
|
||
float32_t HeartValueRes2Peaks[4] = {0.0f};
|
||
float32_t FFT_HeartRateRes[2] = {0.0f};
|
||
float32_t fft_Output(float32_t *Input, int Length, char type)
|
||
{
|
||
float32_t FFT_HeartRateVal[2] = {0};
|
||
memset(FFT_OutPut_Real, 0, sizeof(FFT_OutPut_Real));
|
||
memset(FFT_OutPut, 0, sizeof(FFT_OutPut));
|
||
memset(FFT_InPut, 0, sizeof(FFT_InPut));
|
||
//-----------------------FFT--------------------------------------
|
||
// arm_float_to_q15(testInput_float_50hz, testInput_q15_50hz, FFT_Size);
|
||
uint32_t FFT_Dir = 0;
|
||
uint32_t doBitReverse = 1;
|
||
uint32_t Freq_Index = 0;
|
||
|
||
float32_t res = 0;
|
||
float32_t Freq = 0;
|
||
// FFT初始化
|
||
arm_rfft_fast_instance_f32 FFT_Struct;
|
||
// printf("\r\n-----------------Start---------------------\n");
|
||
/* 初始化结构体S*/
|
||
arm_rfft_fast_init_f32(&FFT_Struct, FFT_Size);
|
||
for (int i = 0; i < FFT_POINT; i++)
|
||
{
|
||
if (i < Length)
|
||
FFT_InPut[i] = Input[i];
|
||
else
|
||
FFT_InPut[i] = 0;
|
||
}
|
||
arm_rfft_fast_f32(&FFT_Struct, FFT_InPut, FFT_OutPut_Real, FFT_Dir);
|
||
/* 实数FFT运算*/
|
||
arm_cmplx_mag_f32(FFT_OutPut_Real, FFT_OutPut, FFT_Size);
|
||
// for (int i = 0; i < FFT_POINT / 2; i++)
|
||
// {
|
||
// printf("%f\r\n", FFT_OutPut[i]);
|
||
// }
|
||
// printf("\r\n-----------------End---------------------\n");
|
||
// 偏置的计算 用于加权
|
||
#if 0
|
||
if (type == TYPE_HEART)
|
||
{
|
||
Freq_Index = MyToolFindMaxIndex(FFT_OutPut, (FFT_POINT / 2));
|
||
Freq = ((float32_t)Freq_Index * 50.0f) / FFT_POINT;
|
||
res = (Freq * 60);
|
||
return res;
|
||
}
|
||
#else
|
||
if (type == TYPE_HEART)
|
||
{
|
||
int valuePeakNum = 0; // 最大的两个有效峰值数 0-2之间
|
||
int maxPeakIndex[2] = {0, 0}; // 用于最有可能的2个索引，最后的结果放在这
|
||
float maxPeakValue[2] = {0.0f}; // 用于存放最大的2个峰值，最后的结果放在这
|
||
valuePeakNum = peakProcess(FFT_OutPut, 200, maxPeakIndex, maxPeakValue);
|
||
// 对不一样滤波器 进行计算
|
||
if (fft_HeartBeatCurrentFilter == FILTER_HI_TYPE)
|
||
stash_Ratio[0] = maxPeakValue[1] / maxPeakValue[0];
|
||
else if (fft_HeartBeatCurrentFilter == FILTER_LO_TYPE)
|
||
stash_Ratio[1] = maxPeakValue[1] / maxPeakValue[0];
|
||
|
||
FFT_HeartRateVal[0] = (((float32_t)maxPeakIndex[0] * 50.0f) / FFT_POINT) * 60;
|
||
FFT_HeartRateVal[1] = (((float32_t)maxPeakIndex[1] * 50.0f) / FFT_POINT) * 60;
|
||
printf("FFT_HeartRateVal [0] : %f", FFT_HeartRateVal[0]);
|
||
printf("FFT_HeartRateVal [1] : %f", FFT_HeartRateVal[1]);
|
||
memcpy(FFT_HeartRateRes, FFT_HeartRateVal, sizeof(FFT_HeartRateVal));
|
||
return 0;
|
||
}
|
||
#endif
|
||
if (type == TYPE_BREATH)
|
||
{
|
||
// valuePeakNum = peakProcess(FFT_OutPut_Accumulate, 33, maxPeakIndex, maxPeakValue);
|
||
Freq_Index = MyToolFindMaxIndex(FFT_OutPut, 33);
|
||
Freq = ((float32_t)Freq_Index * 50.0f) / FFT_POINT;
|
||
res = (Freq * 60);
|
||
return res;
|
||
}
|
||
return 1;
|
||
//----------------------------------------------------------------
|
||
}
|
||
|
||
/**
|
||
* @function:heartDataPostProcessing
|
||
* @brief 心率数据后处理
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void fft_heartDataPostProcessing(float32_t data)
|
||
{
|
||
move_update(f_BPM_hr, 5, (data));
|
||
f_BPM_hr[4] -= (f_BPM_hr[4] <= 90) ? 3 : 4; // 固定offset
|
||
// note: 增加对应的偏置
|
||
#if LINE_COEFF_EN
|
||
if (HeartBeatRate >= 83 && HeartBeatRate <= 110)
|
||
HeartBeatRate = round((HeartBeatRate - 83) * 0.23 + 3 + HeartBeatRate);
|
||
#else
|
||
;
|
||
#endif
|
||
//========================================================================数据后处理
|
||
memcpy(f_BPM_hr2, f_BPM_hr, sizeof(float) * 4);
|
||
if (f_BPM_hr[0] > 0) // 当数据满5个之后开始后处理
|
||
{
|
||
if ((f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) >= 10) || (f_BPM_hr[4] - f_BPM_hr[3] >= 5)) // 比前4个中位数大12或比前面一个大8????????????????????????????????????????????
|
||
{
|
||
count_up_hr += 1; // 判断上升的条件+1,苛刻
|
||
#if FFT_VERSION
|
||
if (count_up_hr >= 12)
|
||
#else
|
||
if (count_up_hr >= 8)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (3 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 3 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+3
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_up_hr >= 6)
|
||
#else
|
||
else if (count_up_hr >= 4)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+2
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + 1;
|
||
}
|
||
count_down_hr = 0;
|
||
count_down_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
else if ((f_BPM_hr[4] - MyToolMidValue(f_BPM_hr2, 4) <= (-8)) || (f_BPM_hr[4] - f_BPM_hr[3] <= (-6))) // 比前4个中位数小8或比前面一个值小6
|
||
{
|
||
count_down_hr += 1;
|
||
#if FFT_VERSION
|
||
if (count_down_hr >= 6)
|
||
#else
|
||
if (count_down_hr >= 4)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_down_hr >= 3)
|
||
#else
|
||
else if (count_down_hr >= 2)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-1) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-1) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3];
|
||
}
|
||
count_up_hr = 0;
|
||
count_up_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
else if (f_BPM_hr[4] - f_BPM_hr[3] > 0)
|
||
{
|
||
if (f_BPM_hr[4] - f_BPM_hr[3] >= 3)
|
||
{
|
||
count_up_hr_m += 1;
|
||
#if FFT_VERSION
|
||
if (count_up_hr + count_up_hr_m >= 10)
|
||
#else
|
||
if (count_up_hr + count_up_hr_m >= 8)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+2
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_up_hr_m >= 8)
|
||
#else
|
||
else if (count_up_hr_m >= 8)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (2 < (f_BPM_hr[4] - f_BPM_hr[3]) ? 2 : (f_BPM_hr[4] - f_BPM_hr[3])); // 最多+2
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_up_hr_m >= 2)
|
||
#else
|
||
else if (count_up_hr_m >= 2)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + 1;
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3];
|
||
}
|
||
}
|
||
count_down_hr = 0;
|
||
count_down_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
// count_up_hr = 0;
|
||
}
|
||
else if (f_BPM_hr[4] - f_BPM_hr[3] < 0)
|
||
{
|
||
if (f_BPM_hr[4] - f_BPM_hr[3] <= (-3))
|
||
{
|
||
count_down_hr_m += 1;
|
||
|
||
#if FFT_VERSION
|
||
if (count_down_hr + count_down_hr_m >= 8)
|
||
#else
|
||
if (count_down_hr + count_down_hr_m >= 8)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_down_hr_m >= 6)
|
||
#else
|
||
else if (count_down_hr_m >= 4)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + ((-2) > (f_BPM_hr[4] - f_BPM_hr[3]) ? (-2) : (f_BPM_hr[4] - f_BPM_hr[3]));
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_down_hr_m >= 2)
|
||
#else
|
||
else if (count_down_hr_m >= 2)
|
||
#endif
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3] + (-1);
|
||
}
|
||
else
|
||
{
|
||
f_BPM_hr[4] = f_BPM_hr[3];
|
||
}
|
||
}
|
||
count_up_hr = 0;
|
||
count_up_hr_m = 0;
|
||
count_out_range_hr = 0;
|
||
}
|
||
if (f_BPM_hr[4] <= 40)
|
||
{
|
||
f_BPM_hr[4] = 40;
|
||
}
|
||
else if (f_BPM_hr[4] > 170)
|
||
{
|
||
f_BPM_hr[4] = 170;
|
||
}
|
||
/**/
|
||
HeartBeatRate = (uint8_t)round(f_BPM_hr[4]);
|
||
// rt_kprintf("------------------- Freq %.6f\r\n", freq_calculated(nf, 0));
|
||
}
|
||
else // 数据没满五个，取平均值,， 且输出值固定在65-75之间
|
||
{
|
||
int Count = 0;
|
||
float sum = 0.0f, avg = 0.0f;
|
||
for (char i = 0; i < 5; i++)
|
||
{
|
||
// rt_kprintf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
|
||
if ((f_BPM_hr[i]) != 0)
|
||
{
|
||
// rt_kprintf("Rand Data :%d\r\n", (rand() % (5)));
|
||
sum += (f_BPM_hr[i]);
|
||
Count++;
|
||
}
|
||
}
|
||
avg = sum / Count;
|
||
HeartBeatRate = (uint8_t)((round(avg)));
|
||
if (HeartBeatRate <= 65)
|
||
{
|
||
HeartBeatRate = 65;
|
||
}
|
||
else if (HeartBeatRate > 75)
|
||
{
|
||
HeartBeatRate = 75;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @function:breathDataPostProcessing
|
||
* @brief 呼吸数据后处理
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
static int hi_counter = 0;
|
||
static int lo_counter = 0;
|
||
void fft_breathDataPostProcessing(float32_t data)
|
||
{
|
||
int temp = round(data);
|
||
if (temp > 9)
|
||
lo_counter = 0;
|
||
if (temp < 24)
|
||
hi_counter = 0;
|
||
if (temp <= 9)
|
||
{
|
||
lo_counter++;
|
||
if (lo_counter <= 40)
|
||
temp = 14;
|
||
if (lo_counter >= 50)
|
||
lo_counter = 50;
|
||
}
|
||
else if (temp >= 24)
|
||
{
|
||
hi_counter++;
|
||
if (hi_counter <= 40)
|
||
temp = 15;
|
||
if (hi_counter >= 50)
|
||
hi_counter = 50;
|
||
}
|
||
|
||
move_update(f_BPM_br, 5, temp);
|
||
// note: 增加对应的偏置
|
||
if (f_BPM_br[4] < 10)
|
||
f_BPM_br[4] = f_BPM_br[4];
|
||
else
|
||
f_BPM_br[4] = f_BPM_br[4] - 0;
|
||
//========================================================================数据后处理
|
||
// if (f_BPM_br[0] > 0 && f_BPM_br2[0] == 0) // 保证最开始的前2个数据可靠性
|
||
// {
|
||
// if (fabs(f_BPM_br[0] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
|
||
// {
|
||
// f_BPM_br[0] = MyToolMidValue(f_BPM_br, 5);
|
||
// }
|
||
// if (fabs(f_BPM_br[1] - MyToolMidValue(f_BPM_br, 5)) > 0.1f)
|
||
// {
|
||
// f_BPM_br[1] = MyToolMidValue(f_BPM_br, 5);
|
||
// }
|
||
// }
|
||
for (int i = 0; i < 4; i++) // 把前4个数据复制出来供后处理分析
|
||
{
|
||
f_BPM_br2[i] = f_BPM_br[i];
|
||
}
|
||
if (f_BPM_br[0] > 0) // 当数据满5个之后开始后处理
|
||
{
|
||
/*
|
||
if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (3) ||
|
||
f_BPM_br[4] - f_BPM_br[3] >= 2) // 比前4个中位数大3
|
||
{
|
||
count_out_range_br += 1;
|
||
if (count_out_range_br > 1)
|
||
{
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
}
|
||
if (count_out_range_br >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(3, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3];
|
||
}
|
||
}
|
||
else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= (-10) ||
|
||
f_BPM_br[4] - f_BPM_br[3] <= (-8)) // 比前4个中位数小8
|
||
{
|
||
count_out_range_br += 1;
|
||
if (count_out_range_br > 1)
|
||
{
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
}
|
||
if (count_out_range_br >= 3)
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-3), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3];
|
||
}
|
||
}
|
||
*/
|
||
// else
|
||
if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) >= (3) ||
|
||
f_BPM_br[4] - f_BPM_br[3] >= 2) // 较大值判断
|
||
{
|
||
count_up_br++;
|
||
#if FFT_VERSION
|
||
if (count_up_br >= 10)
|
||
#else
|
||
if (count_up_br >= 8)
|
||
#endif
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(2.0f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_up_br >= 6)
|
||
#else
|
||
else if (count_up_br >= 4)
|
||
#endif
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(1.0f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3]; // min_compare(0.2f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_out_range_br = 0;
|
||
}
|
||
else if (f_BPM_br[4] - MyToolMidValue(f_BPM_br2, 4) <= ((-3) || f_BPM_br[4] - f_BPM_br[3] <= (-2))) // 较小值判断
|
||
{
|
||
count_down_br++;
|
||
#if FFT_VERSION
|
||
if (count_down_br >= 10)
|
||
#else
|
||
if (count_down_br >= 8)
|
||
#endif
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-2.0f), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
#if FFT_VERSION
|
||
else if (count_down_br >= 6)
|
||
#else
|
||
else if (count_down_br >= 4)
|
||
#endif
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3]; // + max_compare((-0.2f), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
count_out_range_br = 0;
|
||
}
|
||
else if (f_BPM_br[4] - f_BPM_br[3] > 0)
|
||
{
|
||
count_up_br_m++;
|
||
#if FFT_VERSION
|
||
if (count_up_br_m >= 3)
|
||
#else
|
||
if (count_up_br_m >= 3)
|
||
#endif
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + min_compare(1, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3]; // + min_compare(0.3f, (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_down_br = 0;
|
||
count_down_br_m = 0;
|
||
count_out_range_br = 0;
|
||
count_up_br = 0;
|
||
}
|
||
else if (f_BPM_br[4] - f_BPM_br[3] < 0)
|
||
{
|
||
count_down_br_m++;
|
||
#if FFT_VERSION
|
||
if (count_down_br_m >= 3)
|
||
#else
|
||
if (count_down_br_m >= 3)
|
||
#endif
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3] + max_compare((-1), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
else
|
||
{
|
||
f_BPM_br[4] = f_BPM_br[3]; // + max_compare((-0.3f), (f_BPM_br[4] - f_BPM_br[3]));
|
||
}
|
||
count_up_br = 0;
|
||
count_up_br_m = 0;
|
||
count_out_range_br = 0;
|
||
count_down_br = 0;
|
||
}
|
||
if (f_BPM_br[4] < 7)
|
||
{
|
||
f_BPM_br[4] = 7;
|
||
}
|
||
else if (f_BPM_br[4] > 35)
|
||
{
|
||
f_BPM_br[4] = 35;
|
||
}
|
||
/**/
|
||
BreathRate = round(f_BPM_br[4]);
|
||
// rt_kprintf("----------------------------Frq: %.6f\r\n", freq_calculated(nf, 1));
|
||
// rt_kprintf("br-i_max: %d %d %.6f\r\n", i_max_br, BreathRate, freq_calculated(nf, 1));
|
||
}
|
||
else
|
||
{
|
||
int Count = 0;
|
||
float sum, avg = 0.0f;
|
||
for (char i = 0; i < 5; i++)
|
||
{
|
||
// rt_kprintf("f_BPM_hr[%d] = %f\r\n", i, f_BPM_hr[i]);
|
||
if ((f_BPM_br[i]) != 0)
|
||
{
|
||
// rt_kprintf("Rand Data :%d\r\n", (rand() % (5)));
|
||
sum += (f_BPM_br[i]);
|
||
Count++;
|
||
}
|
||
}
|
||
avg = sum / Count;
|
||
BreathRate = (uint8_t)(round(avg));
|
||
if (BreathRate < 11)
|
||
{
|
||
BreathRate = 11;
|
||
}
|
||
else if (BreathRate > 15)
|
||
{
|
||
BreathRate = 15;
|
||
}
|
||
}
|
||
}
|
||
|
||
//------------------------------------------------------------------------------//
|
||
//----------------------------------FFT-----------------------------------------//
|
||
//---------------------------Cooley-Tukey算法-----------------------------------//
|
||
//------------------------------------------------------------------------------//
|
||
struct complex
|
||
{
|
||
double real;
|
||
double image;
|
||
};
|
||
struct complex complex_add(struct complex c1, struct complex c2);
|
||
struct complex complex_sub(struct complex c1, struct complex c2);
|
||
struct complex complex_multi(struct complex c1, struct complex c2);
|
||
struct complex rotation_factor(int N, int n, int k);
|
||
|
||
double mold_length(struct complex c);
|
||
// void fft(int len, struct complex in_x[],struct complex out_y[]);
|
||
void fft(int len, int len_array, struct complex in_x[], float out_y[len_array]);
|
||
#define N 2048
|
||
#define N1 600
|
||
|
||
struct complex
|
||
complex_add(struct complex c1, struct complex c2) // 复数加法
|
||
{
|
||
struct complex p;
|
||
p.real = c1.real + c2.real;
|
||
p.image = c1.image + c2.image;
|
||
return p;
|
||
}
|
||
|
||
struct complex complex_sub(struct complex c1, struct complex c2) // 复数减
|
||
{
|
||
struct complex p;
|
||
p.real = c1.real - c2.real;
|
||
p.image = c1.image - c2.image;
|
||
return p;
|
||
}
|
||
|
||
struct complex complex_multi(struct complex c1, struct complex c2) // 复数乘法
|
||
{
|
||
struct complex c3;
|
||
c3.real = c1.real * c2.real - c1.image * c2.image;
|
||
c3.image = c2.real * c1.image + c1.real * c2.image;
|
||
return c3;
|
||
}
|
||
|
||
struct complex rotation_factor(int Length, int n, int k) // 旋转因子
|
||
{
|
||
struct complex w;
|
||
w.real = cos(2 * PI * n * k / Length);
|
||
w.image = -sin(2 * PI * n * k / Length);
|
||
return w;
|
||
}
|
||
|
||
double mold_length(struct complex c) // 幅度
|
||
{
|
||
return sqrt(c.real * c.real + c.image * c.image);
|
||
};
|
||
|
||
int reverse_num(int l, int oringin_num) // 反位序
|
||
{
|
||
int q = 0, m = 0;
|
||
|
||
for (int k = l - 1; k >= 0; k--)
|
||
{
|
||
q = oringin_num % 2;
|
||
m += q * (1 << k);
|
||
oringin_num = oringin_num / 2;
|
||
}
|
||
return m;
|
||
}
|
||
|
||
/**
|
||
* @function: cooley_tukey_fft
|
||
* @brief: FFT 计算
|
||
* @param {int} len 计算长度
|
||
* @param {int} len_array 数据长度
|
||
* @param {complex} in_x 输入数据
|
||
* @param {float} out_y 输出数据
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
void cooley_tukey_fft(int len, int len_array, struct complex in_x[], float out_y[(len / 2)])
|
||
{
|
||
/*
|
||
param len 序列长度，目前只能是2的指数
|
||
param in_x输入的序列
|
||
param out_y输出的序列
|
||
*/
|
||
int l, k, r, z, dist, q, j; // l是级
|
||
struct complex w, tmp;
|
||
struct complex in_x_mem[len];
|
||
|
||
l = log2(len);
|
||
for (k = 0; k < len; k++)
|
||
{
|
||
in_x_mem[k] = in_x[reverse_num(l, k)]; // 反位序号操作
|
||
}
|
||
for (r = 0; r < l; r++) // 遍历每一级蝶形运算
|
||
{
|
||
dist = 1 << r; // 提前计算每一级的间隔距离
|
||
for (j = 0; j < dist; j++) // 计算策略是拆成上下两组，先上计算，后下计算，j是计算的起始序号
|
||
{
|
||
for (k = j; k < len; k += (dist << 1)) // 不好解释，得画图理解
|
||
{
|
||
q = k + dist; // q同一组蝶形运算第二个序号
|
||
z = k << (l - r - 1); // 确定旋转因子的指数
|
||
w = rotation_factor(len, 1, z);
|
||
// 由于不是并行计算，必须先缓存
|
||
tmp = in_x_mem[k];
|
||
in_x_mem[k] = complex_add(in_x_mem[k], complex_multi(w, in_x_mem[q]));
|
||
in_x_mem[q] = complex_sub(tmp, complex_multi(w, in_x_mem[q]));
|
||
}
|
||
}
|
||
}
|
||
for (int ii = 0; ii < (len * 0.5); ii++)
|
||
{
|
||
out_y[ii] = mold_length(in_x_mem[ii]);
|
||
}
|
||
// memcpy(out_y,in_x_mem,len*sizeof(struct complex));
|
||
}
|
||
|
||
/**
|
||
* @function: myFFT_Output
|
||
* @brief: None
|
||
* @param {float32_t} *Input
|
||
* @param {int} Length
|
||
* @param {char} type
|
||
* @return {*}
|
||
* @author: lzc
|
||
*/
|
||
float32_t myFFT_Output(float32_t *Input, int Length, char type)
|
||
{
|
||
float32_t res = 0;
|
||
float32_t Freq = 0;
|
||
// float32_t y[N1] = {0};
|
||
float32_t *y = malloc(sizeof(float32_t) * 1024);
|
||
|
||
uint32_t Freq_Index = 0;
|
||
struct complex x[N] = {0}; // 初始化为0
|
||
// printf("\n==========Start================\n");
|
||
for (int i = 0; i < N; i++)
|
||
{
|
||
if (i >= 0 && i < Length)
|
||
{
|
||
x[i].real = Input[i];
|
||
x[i].image = 0;
|
||
}
|
||
else
|
||
{
|
||
x[i].real = 0;
|
||
x[i].image = 0;
|
||
}
|
||
}
|
||
cooley_tukey_fft(N, Length, x, y);
|
||
// for (int i = 0; i < N / 2; i++)
|
||
// printf("%f\r\n", y[i]);
|
||
if (type == TYPE_HEART)
|
||
Freq_Index = MyToolFindMaxIndex(y, (FFT_POINT / 2));
|
||
else if (type == TYPE_BREATH)
|
||
Freq_Index = MyToolFindMaxIndex(y, 33);
|
||
Freq = ((float32_t)Freq_Index * 50.0f) / FFT_POINT;
|
||
res = (Freq * 60);
|
||
free(y);
|
||
// printf("\n==========End================\n");
|
||
return res;
|
||
} |