/* * @Description: * @Version: 1.0 * @Author: lzc * @Date: 2023-07-24 09:56:14 * @LastEditors: lzc * @LastEditTime: 2024-12-26 17:01:22 */ #include #include #include #include #include #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" void apply_hanning_window(float32_t *input, float32_t *output, uint32_t data_len, uint32_t FFT_SIZE) { // 生成汉宁窗系数并加窗 for (uint32_t i = 0; i < data_len; i++) { float32_t window = 0.5f * (1 - cosf(2 * PI * i / (data_len - 1))); output[i] = input[i] * window; } // 补零至FFT_SIZE memset(&output[data_len], 0, (FFT_SIZE - data_len) * sizeof(float32_t)); } /** * @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 / 2) + 1] = {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 Energy_Ratio[4] = {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); apply_hanning_window(Input, FFT_InPut, Length, FFT_Size); // 加hanning窗且补零到2048 arm_rfft_fast_f32(&FFT_Struct, FFT_InPut, FFT_OutPut_Real, FFT_Dir); /* 实数FFT运算*/ arm_cmplx_mag_f32(FFT_OutPut_Real, FFT_OutPut, (FFT_POINT / 2) + 1); // 能量修正（乘以1.63） float32_t total_energy = 0.0f; for (int i = 0; i < (FFT_Size / 2) + 1; i++) { FFT_OutPut[i] *= 1.63f; // 能量校准 total_energy += (FFT_OutPut[i]) * (FFT_OutPut[i]); } #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]; Energy_Ratio[0] = (maxPeakValue[0] * maxPeakValue[0]) / total_energy; Energy_Ratio[1] = (maxPeakValue[1] * maxPeakValue[1]) / total_energy; } else if (fft_HeartBeatCurrentFilter == FILTER_LO_TYPE) { stash_Ratio[1] = maxPeakValue[1] / maxPeakValue[0]; Energy_Ratio[2] = (maxPeakValue[0] * maxPeakValue[0]) / total_energy; Energy_Ratio[3] = (maxPeakValue[1] * maxPeakValue[1]) / total_energy; } 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; }