lzc56/Job_SignsPads
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
d1a7b23ea8
Job_SignsPads/STM32/Code/STM32F405/Application/MyBandFitter.c

932 lines
32 KiB
C
Raw Normal View History

[First Push]
2025-04-22 02:29:37 +00:00
#include "MyBandFitter.h"
BandFitterConfig_t BandFitterConfig;
uint32_t blockSize = BLOCK_SIZE;
float32_t FitterOPMData_CheckBed[LENGTH_SAMPLES_IN_OFF_BED] = {0.0f};
float32_t FitterOPMData_Heart[HEART_FITTER_LENGTH] = {0.0f};
float32_t FitterOPMData_Breath[BREATH_FITTER_LENGTH - BREATH_IGNORE_LENGTH] = {0.0f};
float32_t outPutFitterOPMData_Heart[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0.0f};
float32_t outPutFitterOPMData_Breath[BREATH_FITTER_LENGTH - BREATH_IGNORE_LENGTH] = {0.0f};
float32_t outPutFitterOPMData_Heart_Lo[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0.0f};
uint32_t numBlocks = LENGTH_SAMPLES_HEART / BLOCK_SIZE; /* 需要调用arm_biquad_cascade_df1_f32的次数 */
/* 巴特沃斯带通滤波器系数140Hz 400Hz （测试）*/
const float32_t IIRCoeffs32BP[5 * numStages] = {
1.0f, 0.0f, -1.0f, -1.127651872054164616798743736580945551395f,
-0.470013145087532668853214090631809085608f,
1.0f, 0.0f, -1.0f, 0.774953058046049081397654845204669982195f,
-0.367077500556684199750634434167295694351f};
const float32_t IIRs32BPScaleValue[numStages] = {
0.558156585760773649163013487850548699498f,
0.558156585760773649163013487850548699498f};
/* 巴特沃斯带通滤波器系数 0.8Hz 10Hz （心率）*/
const float32_t IIRCoeffs32BP_Heart[5 * numStages] = {
1.0f, 0.0f, -1.0f, 1.904441300281213944600722243194468319416f, -0.912635882040455093111575024522608146071f,
1.0f, 0.0f, -1.0f, 1.626929945180070724575216445373371243477f, -0.721710170252540583746281299681868404150f};
const float32_t IIRs32BPScaleValue_Heart[numStages] = {
0.133977542766808566110370293245068751276, 0.133977542766808566110370293245068751276};
/* 巴特沃斯带通滤波器系数 0.8Hz 10Hz （二次滤波心率参数）*/
const float32_t IIRCoeffs32BP_Heart_NoCut[5 * numStages] = {
1.0f, 0.0f, -1.0f, 1.799723076782722586131058051250874996185f, -0.830942864047083173950625223369570448995f,
1.0f, 0.0f, -1.0f, 1.19173153206713600127386598614975810051f, -0.523449283678365517857855593319982290268f};
const float32_t IIRs32BPScaleValue_Heart_NoCut[numStages] = {
0.246519525895492974676415087742498144507f, 0.246519525895492974676415087742498144507f};
/* 巴特沃斯带通滤波器系数 0.08Hz 2.5Hz （呼吸）*/
const float32_t IIRCoeffs32BP_Breath[5 * numStages] = {
1.0f, 0.0f, -1.0f, 1.985837455305179410913751780753955245018f, -0.985944130217127789883591049147071316838f,
1.0f, 0.0f, -1.0f, 1.583264731971575800528739819128531962633f, -0.659798038006273568356618852703832089901f};
const float32_t IIRs32BPScaleValue_Breath[numStages] = {
0.137599347839688912475963888937258161604f, 0.137599347839688912475963888937258161604f};
// float32_t OPM_Data[LENGTH_SAMPLES_BREATH] = {};
extern float Diff;
extern uint8_t BreathRate;
extern uint8_t HeartBeatRate;
extern uint8_t BreathRate_Record;
extern uint8_t HeartBeatRate_Record;
static float std_calc(float data[], int n);
/**
* @function: arm_iir_f32_bp
* @brief 调用函数arm_iir_f32_bp实现带通滤波器
* @param {float32_t} *Input
* @param {float32_t} *Output
* @param {BandFitterConfig_t} FitterConfig
* @return {*}
* @author: lzc
*/
void arm_iir_f32_bp(float32_t *Input, float32_t *Output, BandFitterConfig_t FitterConfig)
{
uint32_t i;
arm_biquad_casd_df1_inst_f32 S;
float32_t ScaleValue;
float32_t *inputF32, *outputF32;
/* 初始化输入输出缓存指针 */
inputF32 = Input;
outputF32 = Output;
/* 初始化 */
arm_biquad_cascade_df1_init_f32(&S, numStages, (float32_t *)&FitterConfig.IIR_Coeff[0],
(float32_t *)&IIRStateF32[0]);
/* 实现IIR滤波，这里每次处理1个点 */
for (i = 0; i < numBlocks; i++)
{
arm_biquad_cascade_df1_f32(&S, inputF32 + (i * blockSize), outputF32 + (i * blockSize),
blockSize);
}
/*放缩系数 */
ScaleValue = FitterConfig.IIR_ScaleValue[0] * FitterConfig.IIR_ScaleValue[1];
/* 打印滤波后结果 */
for (i = 0; i < LENGTH_SAMPLES; i++)
{
printf("%f\r\n", Input[i]);
}
printf("\r\n-----------------------------------------\n");
for (i = IGNORE_LENGTH; i < LENGTH_SAMPLES; i++)
{
printf("%f\r\n", Output[i] * ScaleValue);
}
}
/**
* @function:
* @brief: None
* @return {*}
* @author: lzc
*/
void arm_iir_f32_bp_stream_Init(BandFitterConfig_t FitterConfig)
{
/* 初始化 */
}
/**
* @function: arm_iir_f32_bp_stream_Output
* @brief: 实现输出
* @param {float32_t} *Input
* @param {float32_t} *Output
* @param {BandFitterConfig_t} FitterConfig
* @return {*}
* @author: lzc
*/
// float32_t DataMaria[400 * 10] = {0.0f};
void arm_iir_f32_bp_stream_Output(float32_t *Input, float32_t *Output, BandFitterConfig_t FitterConfig)
{
uint32_t i;
float32_t Value = 0;
float32_t ScaleValue;
float32_t *inputF32, *outputF32;
arm_biquad_casd_df1_inst_f32 Part;
/* 初始化输入输出缓存指针 */
inputF32 = Input;
outputF32 = Output;
/* 实现IIR滤波，这里每次处理1个点 */
arm_biquad_cascade_df1_init_f32(&Part, numStages, (float32_t *)&FitterConfig.IIR_Coeff[0],
(float32_t *)&IIRStateF32[0]);
for (i = 0; i < numBlocks; i++)
{
arm_biquad_cascade_df1_f32(&Part, inputF32 + (i * blockSize), outputF32 + (i * blockSize),
blockSize);
}
/*放缩系数 */
ScaleValue = FitterConfig.IIR_ScaleValue[0] * FitterConfig.IIR_ScaleValue[1];
// printf("\r\n----------------In----------------------\n");
// for (i = IGNORE_LENGTH; i < LENGTH_SAMPLES; i++)
// {
// Value = Input[i];
// printf("%f\r\n", Input[i]);
// }
// printf("\r\n-----------------------------------------\n");
/* 打印滤波后结果 */
printf("\r\n----------------Out----------------------\n");
for (i = IGNORE_LENGTH; i < LENGTH_SAMPLES; i++)
{
Value = Output[i] * ScaleValue;
if (Value > 0.03f)
Value = 0.03f;
else if (Value < -0.03f)
Value = -0.03f;
printf("%f\r\n", Value);
}
printf("\r\n-----------------------------------------\n");
}
/**
* @function: arm_iir_f32_bp_stream_Heart_Output
* @brief: None
* @param {float32_t} *Input
* @param {float32_t} *Output
* @return {*}
* @author: lzc
*/
arm_biquad_casd_df1_inst_f32 HeartPart;
arm_biquad_casd_df1_inst_f32 HeartPart_NoCut;
void arm_iir_f32_bp_stream_Heart_Output(float32_t *Input, float32_t *Output)
{
float32_t Value = 0;
float32_t ScaleValue;
float32_t *inputF32, *outputF32;
float32_t Output_ResultData[HEART_FITTER_LENGTH] = {0};
// printf("FitterOPMData_Heart In \r\n");
// printf("-----------------------------------------------------\r\n");
for (int i = 0; i < HEART_FITTER_LENGTH; i++)
{
// printf("%f\r\n", FitterOPMData_Heart[i]);
}
// printf("-----------------------------------------------------\r\n");
/*放缩系数 */
ScaleValue = IIRs32BPScaleValue_Heart[0] * IIRs32BPScaleValue_Heart[1];
// 滤波
myFitterLocal_Hr(&HeartPart, Input, Output_ResultData, HEART_FITTER_LENGTH, ScaleValue);
/* 打印滤波后结果 */
// printf("\r\n-------------Heart Out--------------------\n");
for (int i = (HEART_IGNORE_LENGTH); i < HEART_FITTER_LENGTH; i++)
{
Value = Output_ResultData[i];
// if (Value > 0.03f)
// Value = 0.03f;
// else if (Value < -0.03f)
// Value = -0.03f;
Output[i - (HEART_IGNORE_LENGTH)] = Value;
// printf("%f\r\n", Output[i - (HEART_IGNORE_LENGTH)]);
}
// printf("\r\n-----------------------------------------\n");
}
/**
* @function: arm_iir_f32_bp_stream_Heart_Output_NoCut
* @brief: 高频版本
* @param {float32_t} *Input
* @param {float32_t} *Output
* @return {*}
* @author: lzc
*/
void arm_iir_f32_bp_stream_Heart_Output_NoCut(float32_t *Input, float32_t *Output)
{
float32_t Value = 0;
float32_t ScaleValue;
float32_t *inputF32, *outputF32;
float32_t Output_ResultData[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0};
// printf("\r\n-------------Heart In--------------------\n");
for (int i = 0; i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
{
Value = Input[i];
// printf("%f\r\n", Value);
}
// printf("\r\n-----------------------------------------\n");
/*放缩系数 */
ScaleValue = IIRs32BPScaleValue_Heart_NoCut[0] * IIRs32BPScaleValue_Heart_NoCut[1];
// 滤波
myFitterLocal_Hr(&HeartPart_NoCut, Input, Output_ResultData, HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH, ScaleValue);
/* 打印滤波后结果 */
// printf("\r\n-------------Heart Out--------------------\n");
for (int i = (0); i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
{
Value = Output_ResultData[i];
Output[i] = Value;
// printf("%f\r\n", Output[i]);
}
// printf("\r\n-----------------------------------------\n");
}
// 低频版本
void arm_iir_f32_bp_stream_Heart_Output_NoCut_2(float32_t *Input, float32_t *Output)
{
float32_t Value = 0;
float32_t ScaleValue;
float32_t *inputF32, *outputF32;
float32_t Output_ResultData[HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH] = {0};
// printf("\r\n-------------Heart In--------------------\n");
for (int i = 0; i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
{
Value = Input[i];
// printf("%f\r\n", Value);
}
// printf("\r\n-----------------------------------------\n");
/*放缩系数 */
ScaleValue = IIRs32BPScaleValue_Heart[0] * IIRs32BPScaleValue_Heart[1];
// 滤波
myFitterLocal_Hr(&HeartPart, Input, Output_ResultData, HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH, ScaleValue);
/* 打印滤波后结果 */
// printf("\r\n-------------Heart Out--------------------\n");
for (int i = (0); i < HEART_FITTER_LENGTH - HEART_IGNORE_LENGTH; i++)
{
Value = Output_ResultData[i];
Output[i] = Value;
// printf("%f\r\n", Output[i]);
}
// printf("\r\n-----------------------------------------\n");
}
/**
* @function: arm_iir_f32_bp_stream_Breath_Output
* @brief: None
* @param {float32_t} *Input
* @param {float32_t} *Output
* @return {*}
* @author: lzc
*/
extern char equipment_status_value;
arm_biquad_casd_df1_inst_f32 BreathPart;
void arm_iir_f32_bp_stream_Breath_Output(float32_t *Input, float32_t *Output)
{
float32_t Value = 0;
float32_t ScaleValue;
float32_t s_Buffer[400] = {0};
float32_t *inputF32, *outputF32;
float32_t Input_ResultData[BREATH_FITTER_LENGTH] = {0};
float32_t Output_ResultData[BREATH_FITTER_LENGTH] = {0};
for (int i = 0; i < BREATH_FITTER_LENGTH / LENGTH_SAMPLES_BREATH; i++)
{
for (int j = 0; j < LENGTH_SAMPLES_BREATH; j++)
{
Input_ResultData[(LENGTH_SAMPLES_BREATH * i) + j] = Input[j];
}
}
// printf("\r\n-------------Breath 2 IN--------------------\n");
// for (int i = 0; i < LENGTH_SAMPLES_BREATH; i++)
// {
// Input_ResultData[i] = Input[i];
// printf("%f\r\n", Input[i]);
// }
// printf("\r\n-----------------------------------------\n");
/* 初始化输入输出缓存指针 */
inputF32 = Input_ResultData;
outputF32 = Output_ResultData;
for (int i = 0; i < BREATH_FITTER_LENGTH; i++)
{
arm_biquad_cascade_df1_f32(&BreathPart, inputF32 + (i * blockSize), outputF32 + (i * blockSize),
blockSize);
}
/*放缩系数 */
ScaleValue = IIRs32BPScaleValue_Breath[0] * IIRs32BPScaleValue_Breath[1];
/* 打印滤波后结果 */
// printf("\r\n-------------Breath 2 Out--------------------\n");
for (int i = BREATH_IGNORE_LENGTH; i < BREATH_FITTER_LENGTH; i++)
{
Value = Output_ResultData[i] * ScaleValue;
Output[i - BREATH_IGNORE_LENGTH] = Value;
// printf("%f\r\n", Output[i - BREATH_IGNORE_LENGTH]);
}
// printf("\r\n-----------------------------------------\n");
}
/**
* @function: breathInBedCheck
* @brief: V1.9 2024年7月8日
* @param {float32_t} *Input
* @return {*}
* @author: lzc
*/
#define COUNT_BODY_CHECK_VAL 12
#define COUNT_BODY_CHECK_THRESHOLD 8
#define OPM_BODY_CHECK_THRESHOLD 1.0f
#define OPM_BODY_CHECK_HUGE_THRESHOLD 3.5f
#define LEN_Max_array 20 // Max_array长度
float i_count_Std = 0.0f;
float i_count_Max = 0.0f;
float Diff_detail_Past_2 = 0.05f; // Diff_Detail(t-2)
float Diff_detail_Past_1 = 0.05f; // Diff_Detail(t-1)
float Diff_detail_Record = 0.0f; // 记录阶梯值
float FFT_br = 0.0f; // 未经后处理的FFT呼吸输出值
char i_count_No_Breath = 0; // 呼吸暂停指数
char i_count_No_Breath_alarm = 0; // 呼吸暂停预警参数
char i_wait_FFT = 0;
char i_count_off_bed = 0; // 计数Max_array中符合离床要求的max数量
char i_count_no_human = COUNT_BODY_CHECK_VAL;
char i_count_change = 0; // 空床状态下光功大幅变化计数
char i_count_change2 = 0; // 空床状态下光功大幅变化计数
char OPM_Count_2 = 0; // 离床次数
char OPM_CountDown = 0; // 倒计时25秒
char OPM_CountDown_2 = 0; // 在床判断倒计时
char OPM_CountDown_3 = 0; // 倒计时25秒
char OffBed_Flag_Record = 0; // 离床标志记录
float OPM_Current = 0.0f;
float OPM_Record = 0.0f; // 下降沿判断时的光功记录
float OPM_Record_2 = 0.0f; // 记录空床光功
float OPM_Original = 0.0f; // 记录原始空床光功
float OPM_Past_2 = 0.0f; // OPM(t-2)
float OPM_Past_1 = 0.0f; // OPM(t-1)
uint16_t FiveMinutesCount = 0; // 5分钟倒计时 （需要加入到定时器）
float32_t Max_array[LEN_Max_array] = {0};
float32_t TempBuffer[LENGTH_SAMPLES_IN_OFF_BED] = {0};
extern char OffBed_Flag;
extern char FFT_Count_State_Flag;
extern float Diff_Detail;
extern float f_BPM_hr[5];
extern float f_BPM_br[5];
extern void ConvertMoveStd(void);
arm_biquad_casd_df1_inst_f32 CheckPart;
void breathInBedCheck(float32_t *Input)
{
float32_t Max = 0.0f; //
float32_t Std = 0.0f; //
float32_t SumOPM = 0.0f;
float32_t Value = 0;
float32_t ScaleValue;
float32_t s_Buffer[LENGTH_SAMPLES_IN_OFF_BED] = {0};
float32_t *inputF32, *outputF32;
float32_t Output[LENGTH_SAMPLES_IN_OFF_BED] = {0};
float32_t Input_ResultData[BREATH_FITTER_LENGTH] = {0};
float32_t Output_ResultData[BREATH_FITTER_LENGTH] = {0};
float32_t AvgOPM[LENGTH_SAMPLES_IN_OFF_BED / 50] = {0};
// 每50个数作均值
for (int i = 0; i < (LENGTH_SAMPLES_IN_OFF_BED / 50); i++)
{
SumOPM = 0.0f;
for (int j = (i * 50); j < ((i * 50) + 50); j++)
{
SumOPM += FitterOPMData_CheckBed[j];
}
AvgOPM[i] = SumOPM / 50;
}
Max = MyToolFindMaxValue(AvgOPM, 5) - MyToolFindMinValue(AvgOPM, 5);
Std = std_calc(AvgOPM, 5);
i_count_off_bed = 0;
for (int i = 0; i < LEN_Max_array; i++) // 循环更新Max_array，此数组用于离床保险机制
{
if (OffBed_Flag == 0) // 离床状态时数组不起作用
{
Max_array[i] = 0;
}
else
{
if (i == (LEN_Max_array - 1))
{
FFT_br = fft_Output(outPutFitterOPMData_Breath,
BREATH_FITTER_LENGTH - BREATH_IGNORE_LENGTH, TYPE_BREATH);
if ((FFT_br <= 10.0f) || (FFT_br >= 25.0f)) // 此处需计算一次FFTbr，确认无呼吸特征
{
Max_array[i] = Max;
}
else
{
Max_array[i] = 0;
}
}
else
{
Max_array[i] = Max_array[i + 1];
}
if ((Max_array[i] > 0) && (Max_array[i] < 0.001f))
{
i_count_off_bed++;
}
}
}
if ((dbm_value_1310 - OPM_Past_2 < ((-1) * OPM_BODY_CHECK_THRESHOLD)) &&
(OPM_Record_2 < 0) && (OPM_CountDown_2 == 0) && (OPM_Past_2 < 0)) // 检测下降沿并开启在床判断倒计时
{
// 离床情况下进行判断
if (OffBed_Flag == 0)
{
OPM_CountDown = 0; // 离床倒计时index
OPM_Record = OPM_Past_2;
OPM_CountDown_2 = 25; // 在床倒计时index
}
}
if ((dbm_value_1310 - OPM_Past_2 > (OPM_BODY_CHECK_THRESHOLD)) && (OffBed_Flag == 4)) // 检测上升沿
{
FiveMinutesCount = 120;
}
if ((OffBed_Flag == 0) && (Max < 0.002f)) // 记录空床光功
{
if (OPM_CountDown_2 == 0) // 进行在床倒计时（即在床判断过程中）不记录空床光功
{
if (OPM_Record_2 < 0)
{
if (fabs(OPM_Record_2 - dbm_value_1310) > 0.3) // 当空床状态下光功数值发生大幅变化时,连续检测到3次变化再记录新空床光功
{
i_count_change++;
if (i_count_change == 10)
{
OPM_Record_2 = dbm_value_1310;
}
}
else
{
i_count_change = 0;
OPM_Record_2 = dbm_value_1310;
}
}
else // 初次赋值空床光功
{
OPM_Record_2 = dbm_value_1310;
}
// NOTE:初次上电赋值
if (OPM_Original == -10.00f) // 上电时在稳定离床状态下记录一次初始光功
{
OPM_Original = OPM_Record_2;
printf("Ready To Write %f", OPM_Original); // 记录original专用
EN25QXX_Write((uint8_t *)&OPM_Original,
FLASH_IN_OFF_BED_OPM_RECORD_ADDRESS,
sizeof(float32_t));
}
if (fabs(OPM_Record_2 - OPM_Original) > 0.3) // 当空床状态下光功数值发生大幅变化时,连续检测到25次变化再更新初始光功
{
i_count_change2++;
if (i_count_change2 == 25)
{
OPM_Original = OPM_Record_2;
printf("Ready To Write %f", OPM_Original); // 记录original专用
EN25QXX_Write((uint8_t *)&OPM_Original,
FLASH_IN_OFF_BED_OPM_RECORD_ADDRESS,
sizeof(float32_t));
}
}
else
{
i_count_change2 = 0;
}
}
}
else // 不满足条件时将count指数清零
{
i_count_change = 0;
i_count_change2 = 0;
}
if (OPM_Record_2 == 0.0f) // 记录original版专用
{
OPM_Record_2 = OPM_Original;
}
if ((OffBed_Flag == 4) && (Diff_Detail < 0.03f)) // 在床状态下检测Diff_detail下降沿
{
if (((Diff_Detail - Diff_detail_Past_2) < (-0.5 * Diff_detail_Past_2)) && (i_count_No_Breath == 0))
{
Diff_detail_Record = Diff_detail_Past_2;
}
}
if ((Diff_detail_Record > 0) && ((Diff_Detail - Diff_detail_Record) < (-0.5 * Diff_detail_Record))) // 检测下降沿保持时间
{
if (OffBed_Flag == 4)
{
i_count_No_Breath++;
}
//----------------------------------------------------------------
FFT_br = fft_Output(outPutFitterOPMData_Breath,
BREATH_FITTER_LENGTH - BREATH_IGNORE_LENGTH, TYPE_BREATH);
if (i_count_No_Breath >= 10) // 当呼吸暂停指数大于阈值时开始检测呼吸率频谱是否满足暂停要求
{
if ((FFT_br <= 7.0f) || (FFT_br >= 45.0f)) // 若满足呼吸率频谱要求则将指数增加，使得呼吸置零能保持一小段时间
{
i_count_No_Breath = i_count_No_Breath + 2;
i_wait_FFT = 0;
}
else // 若不满足要求则将指数减1，等待频谱满足要求
{
i_count_No_Breath--;
i_wait_FFT++;
if (i_wait_FFT >= 4) // 只等待频谱固定时间，防止FFT错误数据影响判断。
{
i_wait_FFT = 0;
i_count_No_Breath = 0;
}
}
if (i_count_No_Breath >= 10) // 当频谱满足要求后指数也要逐次递减，在频谱偶尔满足要求的情况下能够减少误报暂停
{
i_count_No_Breath--;
i_wait_FFT = 0;
if ((FFT_br >= 7.0f) && (FFT_br <= 45.0f)) // 若不满足呼吸率频谱要求则将指数置零
{
i_count_No_Breath = 0;
}
}
}
if (i_count_No_Breath >= 15) // 限制参数不要过大，在出现误报后能迅速修正
{
i_count_No_Breath = 15;
}
}
else
{
Diff_detail_Record = 0;
i_count_No_Breath = 0;
}
//----------------------------------------------------------------
if (i_count_No_Breath >= 13) // 当呼吸暂停指数大于阈值时。将呼吸置零，并开始计数呼吸暂停预警参数
{
// BR = 0; // 呼吸置零
FFT_Count_State_Flag = FFT_No_breath; //
i_count_No_Breath_alarm++;
}
else
{
i_count_No_Breath_alarm = 0;
}
if (i_count_No_Breath_alarm >= 60) // 当呼吸暂停预警参数大于阈值时，上报一次预警并将该参数重新置零
{
// No_Breath_alarm = 1; // 呼吸暂停预警
i_count_No_Breath_alarm = 0;
}
if (Diff_Detail < 0.25f) // 只在相对静止情况下更新Diff_detail相关的记录值
{
Diff_detail_Past_2 = Diff_detail_Past_1;
Diff_detail_Past_1 = Diff_Detail;
}
// StdRecord = Std;
// printf("\r\n-----------------------------------------\n");
// printf("dbm_value_1310 %f\r\n", dbm_value_1310);
// printf("MAX %f\r\n", Max);
// printf("Diff %f\r\n", Diff);
// printf("i_count is %d\r\n", i_count_no_human);
// printf("i_count_off_bed is %d\r\n", i_count_off_bed);
// printf("OPM_Record %f\r\n", OPM_Record);
// printf("OPM_Record_2 %f\r\n", OPM_Record_2);
// printf("OPM_CountDown_2 %d\r\n", OPM_CountDown_2);
// printf("\r\n-----------------------------------------\n");
if (OffBed_Flag == 4) // 在床状态前提下进行离床判断
{
#if SPEED_UP_MODE_EN
if ((dbm_value_1310 - OPM_Record_2 > -0.9f) && (Max < 150.0f)) // 若和空床光功连续接近6s则判为离床
{
OPM_CountDown_3++;
if (OPM_CountDown_3 >= 3)
{
OPM_Count_2 = 0;
i_count_no_human = 12;
}
}
#else
if ((dbm_value_1310 - OPM_Record_2 > -0.5f) && (Max < 1.5f)) // 若和空床光功连续接近6s则判为离床
{
OPM_CountDown_3++;
if (OPM_CountDown_3 >= 6)
{
OPM_Count_2 = 0;
i_count_no_human = 12;
}
}
#endif
else
{
OPM_CountDown_3 = 0;
}
// DONE: 增加时间判断条件
if ((i_count_off_bed >= round(0.8 * LEN_Max_array)) &&
((OPM_Record_2 == 0) || (dbm_value_1310 - OPM_Record_2 > -1.5f))) // 新离床判断的保险机制，追溯长度中80%满足离床条件即判为离床
{
i_count_no_human = 12;
}
}
else if (OffBed_Flag == 0) // 离床状态前提下进行在床判断
{
if ((OPM_CountDown_2 > 0) && (OPM_Record < 0) && (OPM_Record_2 < 0)) // 在倒计时范围内，持续检测到下降沿台阶，进行在床判断
{
OPM_CountDown_2--;
if ((dbm_value_1310 - OPM_Record < ((-1) * OPM_BODY_CHECK_THRESHOLD)))
{
OPM_CountDown_3 = 0;
if ((Max > 0.003f))
{
i_count_no_human--;
if (i_count_no_human <= 0)
i_count_no_human = 0;
}
}
}
else if (OPM_CountDown_2 == 0) // 若倒计时结束还在离床状态，将索引恢复
{
i_count_no_human = 12;
}
if ((Max > 0.009f)) // 在床判断的保险机制，连续30s有体征则判为在床
{
OPM_Count_2++;
OPM_CountDown_3 = 0;
if (OPM_Count_2 >= 60)
{
OPM_Count_2 = 60;
i_count_no_human = 0;
OPM_Record_2 = OPM_Original; // 防止空床光功误报，将其初始化
}
}
else
{
OPM_Count_2 = 0;
}
if (i_count_no_human < COUNT_BODY_CHECK_THRESHOLD) // 已经进入在床状态后将倒计时关闭
{
OPM_CountDown_2 = 0;
}
}
// 光功正常 开始判断在离床
if (dbm_value_1310 > WEAK_OPM && dbm_value_1310 < HI_OPM)
{
if (i_count_no_human >= COUNT_BODY_CHECK_THRESHOLD)
{
OffBed_Flag = 0; // 离床
FiveMinutesCount = 0;
// 可屏蔽用于一直判断离床
equipment_status_value = OffBed_Status;
i_count_No_Breath = 0;
if (OffBed_Flag_Record == 4)
{
// NOTE: 清除缓存
BreathRate = 0;
HeartBeatRate = 0;
memset(&fft_HeartBeatRate_StashBuffer, 0,
sizeof(fft_HeartBeatRate_StashBuffer));
// BreathRate_Record = 0;
// HearDtBeaFtRate_Record = 0;
memset(&f_BPM_hr, 0, sizeof(f_BPM_hr));
memset(&f_BPM_br, 0, sizeof(f_BPM_br));
}
}
else if (i_count_no_human < COUNT_BODY_CHECK_THRESHOLD)
{
OffBed_Flag = 4; // 在床
ConvertMoveStd();
OPM_CountDown_2 = 0;
equipment_status_value = InBed_Status;
}
}
i_count_Std = Std;
i_count_Max = Max;
OPM_Past_2 = OPM_Past_1; //
OPM_Past_1 = OPM_Current; //
OPM_Current = dbm_value_1310;
memcpy(TempBuffer, FitterOPMData_CheckBed, sizeof(FitterOPMData_CheckBed));
memset(FitterOPMData_CheckBed, 0, sizeof(FitterOPMData_CheckBed));
for (int i = 0; i < 200; i++)
FitterOPMData_CheckBed[i] = TempBuffer[i + 50];
OffBed_Flag_Record = OffBed_Flag;
// printf("-----------------------------------------------------InOff End\n");
}
/**
* @function: std_calc
* @brief: None
* @param {float} data
* @param {int} n
* @return {*}
* @author: lzc
*/
static float std_calc(float data[], int n)
{
float sum = 0.0, mean, standardDeviation = 0.0;
int i;
for (i = 0; i < n; i++)
{
sum += data[i];
}
mean = sum / n;
for (i = 0; i < n; i++)
standardDeviation += pow(data[i] - mean, 2);
return sqrt(standardDeviation / n);
}
// 函数定义：将固定长度的一维数组倒置
void reverseArray(float *arr, int length)
{
int start = 0;
int end = length - 1;
// 交换数组元素直到中间位置
while (start < end)
{
// 交换 arr[start] 和 arr[end]
float temp = arr[start];
arr[start] = arr[end];
arr[end] = temp;
// 移动索引
start++;
end--;
}
}
/**
* @function:myFitterLocal_Hr
* @brief: None
* @return {*}
* @author: lzc
*/
void myFitterLocal_Hr(const arm_biquad_casd_df1_inst_f32 *S,
float32_t *Input, float32_t *Output, int Length, float ScaleValue)
{
float Value = 0.0f;
// float ScaleValue = 0.0f; //
float left_ext[200] = {0};
float right_ext[200] = {0};
float ext[HEART_FITTER_LENGTH + (200 * 2)] = {0};
float ext_output_1[HEART_FITTER_LENGTH + (200 * 2)] = {0};
// rt_memset(ext, 0, sizeof(ext));
// rt_memset(left_ext, 0, sizeof(left_ext));
// rt_memset(right_ext, 0, sizeof(right_ext));
// rt_memset(ext_output_1, 0, sizeof(ext_output_1));
// 取X的第一个数为left_end
float left_end = Input[0];
// 按顺序取X第28至第2个数，存为数组left_ext
for (int i = 1; i <= 200; i++)
left_ext[i - 1] = Input[(200 + 1) - i];
// 取X的最后一个数为right_end
float right_end = Input[Length - 1];
// 按顺序取X倒数第2个数至倒数第28个数，存为数组right_ext
for (int i = 1; i <= 200; i++)
{
right_ext[i - 1] = Input[Length - 1 - i];
}
// 将数组（2*left_end - left_ext）拼接在X的头端
for (int i = 1; i <= 200; i++)
{
ext[i - 1] = left_ext[i - 1];
}
for (int i = 0; i < Length; i++)
{
ext[i + 200] = Input[i];
// dbg_printf("%f\n", Input[i]);
}
// 将数组（2*right_end - right_ext）拼接在X的尾端
for (int i = 0; i < 200; i++)
{
ext[Length + 200 + i] = (right_ext[i]);
}
// 将ext进行一次滤波
for (int i = 0; i < (Length + (200 * 2)); i++)
arm_biquad_cascade_df1_f32(S, ext + (i * blockSize), ext_output_1 + (i * blockSize), blockSize);
/* 打印滤波后结果 */
for (int i = 0; i < (Length + (200 * 2)); i++)
{
Value = ext_output_1[i] * ScaleValue;
ext_output_1[i] = Value;
}
// 假设第一次滤波后的数组为ext,将滤波后的数组倒置
reverseArray(ext_output_1, (Length + (200 * 2)));
// 将倒置后的数组ext再次滤波
for (int i = 0; i < (Length + (200 * 2)); i++)
arm_biquad_cascade_df1_f32(S, ext_output_1 + (i * blockSize), ext + (i * blockSize), blockSize);
/* 打印滤波后结果 */
for (int i = 0; i < (Length + (2 * 200)); i++)
{
Value = ext[i] * ScaleValue;
ext[i] = Value;
}
// 将第二次滤波后的数组ext再次倒置
reverseArray(ext, (Length + (200 * 2)));
// 截取滤波后的X_filtered
for (int i = 0; i < Length; i++)
{
Output[i] = ext[i + 200];
}
}
/**
* @function:myFitterLocal_Br
* @brief: None
* @return {*}
* @author: lzc
*/
void myFitterLocal_Br(const arm_biquad_casd_df1_inst_f32 *S,
float32_t *Input, float32_t *Output, int Length, float ScaleValue)
{
float Value = 0.0f;
// float ScaleValue = 0.0f; //
float left_ext[400] = {0};
float right_ext[400] = {0};
float ext[HEART_FITTER_LENGTH + (400 * 2)] = {0};
float ext_output_1[HEART_FITTER_LENGTH + (400 * 2)] = {0};
// rt_memset(ext, 0, sizeof(ext));
// rt_memset(left_ext, 0, sizeof(left_ext));
// rt_memset(right_ext, 0, sizeof(right_ext));
// rt_memset(ext_output_1, 0, sizeof(ext_output_1));
// 取X的第一个数为left_end
float left_end = Input[0];
// 按顺序取X第28至第2个数，存为数组left_ext
for (int i = 1; i <= 400; i++)
left_ext[i - 1] = Input[(400 + 1) - i];
// 取X的最后一个数为right_end
float right_end = Input[Length - 1];
// 按顺序取X倒数第2个数至倒数第28个数，存为数组right_ext
for (int i = 1; i <= 400; i++)
{
right_ext[i - 1] = Input[Length - 1 - i];
}
// 将数组（2*left_end - left_ext）拼接在X的头端
for (int i = 1; i <= 400; i++)
{
ext[i - 1] = left_ext[i - 1];
}
for (int i = 0; i < Length; i++)
{
ext[i + 400] = Input[i];
// dbg_printf("%f\n", Input[i]);
}
// 将数组（2*right_end - right_ext）拼接在X的尾端
for (int i = 0; i < 400; i++)
{
ext[Length + 400 + i] = (right_ext[i]);
}
// 将ext进行一次滤波
for (int i = 0; i < (Length + (400 * 2)); i++)
arm_biquad_cascade_df1_f32(&HeartPart, ext + (i * blockSize), ext_output_1 + (i * blockSize), blockSize);
/* 打印滤波后结果 */
for (int i = 0; i < (Length + (400 * 2)); i++)
{
Value = ext_output_1[i] * ScaleValue;
ext_output_1[i] = Value;
}
// 假设第一次滤波后的数组为ext,将滤波后的数组倒置
reverseArray(ext_output_1, (Length + (400 * 2)));
// 将倒置后的数组ext再次滤波
for (int i = 0; i < (Length + (400 * 2)); i++)
arm_biquad_cascade_df1_f32(&HeartPart, ext_output_1 + (i * blockSize), ext + (i * blockSize), blockSize);
/* 打印滤波后结果 */
for (int i = 0; i < (Length + (2 * 400)); i++)
{
Value = ext[i] * ScaleValue;
ext[i] = Value;
}
// 将第二次滤波后的数组ext再次倒置
reverseArray(ext, (Length + (400 * 2)));
// 截取滤波后的X_filtered
for (int i = 0; i < Length; i++)
{
Output[i] = ext[i + 400];
}
}
Reference in New Issue Copy Permalink