216 lines
7.3 KiB
C
216 lines
7.3 KiB
C
|
|
||
|
|
#include <main.h>
|
||
|
|
#include <math.h>
|
||
|
|
#include "cwt.h"
|
||
|
|
#include "string.h"
|
||
|
|
#include "stdio.h"
|
||
|
|
#include "MyBandFitter.h"
|
||
|
|
#include "MyAlgorithm.h"
|
||
|
|
|
||
|
|
extern int FitterCount_CheckBed;
|
||
|
|
extern char FitterCollectDoneFlag_CheckBed;
|
||
|
|
|
||
|
|
extern float ampSum[HEART_ARRAY_LENGTH];
|
||
|
|
extern float ampSumRr[BREATH_ARRAY_LENGTH];
|
||
|
|
/************************ PRIVATE ************************/
|
||
|
|
|
||
|
|
void convolve(const float Signal[/* SignalLen */], int SignalLen,
|
||
|
|
const float Kernel[/* KernelLen */], int KernelLen,
|
||
|
|
float Result[/* SignalLen + KernelLen - 1 */])
|
||
|
|
{
|
||
|
|
int n;
|
||
|
|
for (n = 0; n < SignalLen + KernelLen - 1; n++)
|
||
|
|
{
|
||
|
|
int kmin, kmax, k;
|
||
|
|
Result[n] = 0;
|
||
|
|
kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
|
||
|
|
kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
|
||
|
|
for (k = kmin; k <= kmax; k++)
|
||
|
|
{
|
||
|
|
Result[n] += Signal[k] * Kernel[n - k];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/************************ PUBLIC ************************/
|
||
|
|
|
||
|
|
int cwt_hr(float *s, int n, float amp[][n])
|
||
|
|
{
|
||
|
|
/* variables for computing cwt */
|
||
|
|
float a;
|
||
|
|
double stepp = 0.009775171065493637;
|
||
|
|
for (int dy = 943; dy < 988; dy++)
|
||
|
|
{
|
||
|
|
a = (2 * 0.7 * 1024) / (1024 - dy);
|
||
|
|
int len_jj = ceil(10 * a) + 1;
|
||
|
|
// int jj[len_jj];
|
||
|
|
int *jj = (int *)malloc(len_jj * sizeof(int));
|
||
|
|
for (int i = 0; i < len_jj; i++)
|
||
|
|
{
|
||
|
|
jj[i] = floor(i / (a * stepp));
|
||
|
|
}
|
||
|
|
int len_jjj = len_jj;
|
||
|
|
if (jj[len_jj - 1] >= 1024)
|
||
|
|
{
|
||
|
|
for (int i = len_jj - 1; i >= 0; i--)
|
||
|
|
{
|
||
|
|
if (jj[i] < 1024)
|
||
|
|
{
|
||
|
|
len_jjj = i + 1;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
free(jj);
|
||
|
|
// int jjj[len_jjj];
|
||
|
|
// float int_psi_scale_real[len_jjj];
|
||
|
|
// float int_psi_scale_imag[len_jjj];
|
||
|
|
int *jjj = (int *)malloc(len_jjj * sizeof(int));
|
||
|
|
float *int_psi_scale_real = (float *)malloc(len_jjj * sizeof(float));
|
||
|
|
float *int_psi_scale_imag = (float *)malloc(len_jjj * sizeof(float));
|
||
|
|
for (int i = 0; i < len_jjj; i++)
|
||
|
|
{
|
||
|
|
jjj[i] = jj[i];
|
||
|
|
int_psi_scale_real[len_jjj - 1 - i] = int_psi_real[jjj[i]];
|
||
|
|
int_psi_scale_imag[len_jjj - 1 - i] = int_psi_imag[jjj[i]];
|
||
|
|
}
|
||
|
|
float result_r[(n + len_jjj - 1)];
|
||
|
|
float result_i[(n + len_jjj - 1)];
|
||
|
|
if (n > len_jjj)
|
||
|
|
{
|
||
|
|
convolve(s, n, int_psi_scale_real, len_jjj, result_r);
|
||
|
|
convolve(s, n, int_psi_scale_imag, len_jjj, result_i);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
convolve(int_psi_scale_real, len_jjj, s, n, result_r);
|
||
|
|
convolve(int_psi_scale_imag, len_jjj, s, n, result_i);
|
||
|
|
}
|
||
|
|
free(jjj);
|
||
|
|
free(int_psi_scale_real);
|
||
|
|
free(int_psi_scale_imag);
|
||
|
|
float SqrtValue_a = 0.0f; //
|
||
|
|
float PowSqrtValue = 0.0f; //
|
||
|
|
// float diff_conv_r[n + len_jjj - 2];
|
||
|
|
// float diff_conv_i[n + len_jjj - 2];
|
||
|
|
float *diff_conv_r = (float *)malloc((n + len_jjj - 2) * sizeof(float));
|
||
|
|
float *diff_conv_i = (float *)malloc((n + len_jjj - 2) * sizeof(float));
|
||
|
|
arm_sqrt_f32(a, &(SqrtValue_a));
|
||
|
|
for (int i = 1; i < (n + len_jjj - 1); i++)
|
||
|
|
{
|
||
|
|
diff_conv_r[i - 1] = (result_r[i] - result_r[i - 1]) * (-1 * SqrtValue_a);
|
||
|
|
diff_conv_i[i - 1] = (result_i[i] - result_i[i - 1]) * (-1 * SqrtValue_a);
|
||
|
|
}
|
||
|
|
float cut_dim = (float)(len_jjj - 2) / 2;
|
||
|
|
for (int i = floor(cut_dim); i < (n + len_jjj - 2 - ceil(cut_dim)); i++)
|
||
|
|
{
|
||
|
|
arm_sqrt_f32(pow(diff_conv_r[i], 2.0) + pow(diff_conv_i[i], 2.0), &PowSqrtValue);
|
||
|
|
// amp[dy - 942][i - (int)floor(cut_dim)] = PowSqrtValue;
|
||
|
|
ampSum[dy - 943] += PowSqrtValue;
|
||
|
|
}
|
||
|
|
free(diff_conv_r);
|
||
|
|
free(diff_conv_i);
|
||
|
|
get_optical_power();
|
||
|
|
get_check_result();
|
||
|
|
if (FitterCollectDoneFlag_CheckBed)
|
||
|
|
{
|
||
|
|
FitterCollectDoneFlag_CheckBed = 0;
|
||
|
|
breathInBedCheck(FitterOPMData_CheckBed);
|
||
|
|
memcpy(FitterOPMData_CheckBed, FitterOPMData_CheckBed + 50,
|
||
|
|
sizeof(float32_t) * (LENGTH_SAMPLES_IN_OFF_BED - 50));
|
||
|
|
FitterCount_CheckBed = LENGTH_SAMPLES_IN_OFF_BED - 50;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int cwt_rr(float *s, int n, float amp[][n])
|
||
|
|
{
|
||
|
|
/* variables for computing cwt */
|
||
|
|
float a = 0.0f;
|
||
|
|
double stepp = 0.009775171065493637;
|
||
|
|
for (int dy = 1004; dy < 1016; dy++)
|
||
|
|
{
|
||
|
|
a = (2 * 0.7 * 1024) / (1024 - dy);
|
||
|
|
int len_jj = ceil(10 * a) + 1;
|
||
|
|
|
||
|
|
// int jj[len_jj];
|
||
|
|
int *jj = (int *)malloc(len_jj * sizeof(int));
|
||
|
|
for (int i = 0; i < len_jj; i++)
|
||
|
|
{
|
||
|
|
jj[i] = floor(i / (a * stepp));
|
||
|
|
}
|
||
|
|
int len_jjj = len_jj;
|
||
|
|
if (jj[len_jj - 1] >= 1024)
|
||
|
|
{
|
||
|
|
for (int i = len_jj - 1; i >= 0; i--)
|
||
|
|
{
|
||
|
|
if (jj[i] < 1024)
|
||
|
|
{
|
||
|
|
len_jjj = i + 1;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
free(jj);
|
||
|
|
|
||
|
|
// int jjj[len_jjj];
|
||
|
|
// float int_psi_scale_real[len_jjj];
|
||
|
|
// float int_psi_scale_imag[len_jjj];
|
||
|
|
|
||
|
|
int *jjj = (int *)malloc(len_jjj * sizeof(int));
|
||
|
|
float *int_psi_scale_real = (float *)malloc(len_jjj * sizeof(float));
|
||
|
|
float *int_psi_scale_imag = (float *)malloc(len_jjj * sizeof(float));
|
||
|
|
for (int i = 0; i < len_jjj; i++)
|
||
|
|
{
|
||
|
|
jjj[i] = jj[i];
|
||
|
|
int_psi_scale_real[len_jjj - 1 - i] = int_psi_real[jjj[i]];
|
||
|
|
int_psi_scale_imag[len_jjj - 1 - i] = int_psi_imag[jjj[i]];
|
||
|
|
}
|
||
|
|
float result_r[(n + len_jjj - 1)];
|
||
|
|
float result_i[(n + len_jjj - 1)];
|
||
|
|
if (n > len_jjj)
|
||
|
|
{
|
||
|
|
convolve(s, n, int_psi_scale_real, len_jjj, result_r);
|
||
|
|
convolve(s, n, int_psi_scale_imag, len_jjj, result_i);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
convolve(int_psi_scale_real, len_jjj, s, n, result_r);
|
||
|
|
convolve(int_psi_scale_imag, len_jjj, s, n, result_i);
|
||
|
|
}
|
||
|
|
free(jjj);
|
||
|
|
free(int_psi_scale_real);
|
||
|
|
free(int_psi_scale_imag);
|
||
|
|
|
||
|
|
float SqrtValue_a = 0.0f; //
|
||
|
|
float PowSqrtValue = 0.0f; //
|
||
|
|
// float diff_conv_r[n + len_jjj - 2];
|
||
|
|
// float diff_conv_i[n + len_jjj - 2];
|
||
|
|
float *diff_conv_r = (float *)malloc((n + len_jjj - 2) * sizeof(float));
|
||
|
|
float *diff_conv_i = (float *)malloc((n + len_jjj - 2) * sizeof(float));
|
||
|
|
arm_sqrt_f32(a, &(SqrtValue_a));
|
||
|
|
for (int i = 1; i < (n + len_jjj - 1); i++)
|
||
|
|
{
|
||
|
|
diff_conv_r[i - 1] = (result_r[i] - result_r[i - 1]) * (-1 * SqrtValue_a);
|
||
|
|
diff_conv_i[i - 1] = (result_i[i] - result_i[i - 1]) * (-1 * SqrtValue_a);
|
||
|
|
}
|
||
|
|
float cut_dim = (float)(len_jjj - 2) / 2;
|
||
|
|
for (int i = floor(cut_dim); i < (n + len_jjj - 2 - ceil(cut_dim)); i++)
|
||
|
|
{
|
||
|
|
arm_sqrt_f32(pow(diff_conv_r[i], 2.0) + pow(diff_conv_i[i], 2.0), &PowSqrtValue);
|
||
|
|
ampSumRr[dy - 1004] += PowSqrtValue;
|
||
|
|
}
|
||
|
|
free(diff_conv_r);
|
||
|
|
free(diff_conv_i);
|
||
|
|
get_optical_power();
|
||
|
|
get_check_result();
|
||
|
|
// if (FitterCollectDoneFlag_CheckBed)
|
||
|
|
// {
|
||
|
|
// FitterCount_CheckBed = 0;
|
||
|
|
// FitterCollectDoneFlag_CheckBed = 0;
|
||
|
|
// breathInBedCheck(FitterOPMData_CheckBed);
|
||
|
|
// }
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|