Add DC removing to signal power measurement.
This commit is contained in:
Haruki NAOI
committed by
shono.takafumi
shono.takafumi
parent
cb927c7f9a
commit
39b9ffd69b
@@ -29,6 +29,7 @@
|
|||||||
|
|
||||||
#define shift 4
|
#define shift 4
|
||||||
//#define shift_DC 0
|
//#define shift_DC 0
|
||||||
|
#define SHRT_MIN -32768
|
||||||
|
|
||||||
#if defined(__x86_64__) || defined(__i386__)
|
#if defined(__x86_64__) || defined(__i386__)
|
||||||
#ifdef LOCALIZATION
|
#ifdef LOCALIZATION
|
||||||
@@ -67,52 +68,86 @@ int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_en
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
//int32_t signal_energy(int32_t *input,uint32_t length)
|
||||||
|
//{
|
||||||
|
//
|
||||||
|
// int32_t i;
|
||||||
|
// int32_t temp,temp2;
|
||||||
|
// register __m64 mm0,mm1,mm2,mm3;
|
||||||
|
// __m64 *in = (__m64 *)input;
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
|
||||||
|
// mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
|
||||||
|
//
|
||||||
|
// for (i=0; i<length>>1; i++) {
|
||||||
|
//
|
||||||
|
// mm1 = in[i];
|
||||||
|
// mm2 = mm1;
|
||||||
|
// mm1 = _m_pmaddwd(mm1,mm1);
|
||||||
|
// mm1 = _m_psradi(mm1,shift);// shift any 32 bits blocs of the word by the value shift
|
||||||
|
// mm0 = _m_paddd(mm0,mm1);// add the two 64 bits words 4 bytes by 4 bytes
|
||||||
|
// // mm2 = _m_psrawi(mm2,shift_DC);
|
||||||
|
// mm3 = _m_paddw(mm3,mm2);// add the two 64 bits words 2 bytes by 2 bytes
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// mm1 = mm0;
|
||||||
|
// mm0 = _m_psrlqi(mm0,32);
|
||||||
|
// mm0 = _m_paddd(mm0,mm1);
|
||||||
|
// temp = _m_to_int(mm0);
|
||||||
|
// temp/=length;
|
||||||
|
// temp<<=shift; // this is the average of x^2
|
||||||
|
//
|
||||||
|
// // now remove the DC component
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// mm2 = _m_psrlqi(mm3,32);
|
||||||
|
// mm2 = _m_paddw(mm2,mm3);
|
||||||
|
// mm2 = _m_pmaddwd(mm2,mm2);
|
||||||
|
// temp2 = _m_to_int(mm2);
|
||||||
|
// temp2/=(length*length);
|
||||||
|
// // temp2<<=(2*shift_DC);
|
||||||
|
// temp -= temp2;
|
||||||
|
//
|
||||||
|
// _mm_empty();
|
||||||
|
// _m_empty();
|
||||||
|
//
|
||||||
|
// return((temp>0)?temp:1);
|
||||||
|
|
||||||
|
//-----------------------------------------------------------------
|
||||||
|
// Average Power calculation with DC removing
|
||||||
|
//-----------------------------------------------------------------
|
||||||
int32_t signal_energy(int32_t *input,uint32_t length)
|
int32_t signal_energy(int32_t *input,uint32_t length)
|
||||||
{
|
{
|
||||||
|
uint32_t i;
|
||||||
|
int32_t temp;
|
||||||
|
__m128i in, in_clp, i16_min, coe1;
|
||||||
|
__m128 num0, num1, num2, num3, recp1;
|
||||||
|
|
||||||
int32_t i;
|
//init
|
||||||
int32_t temp,temp2;
|
num0 = _mm_setzero_ps();
|
||||||
register __m64 mm0,mm1,mm2,mm3;
|
num1 = _mm_setzero_ps();
|
||||||
__m64 *in = (__m64 *)input;
|
i16_min = _mm_set1_epi16(SHRT_MIN);
|
||||||
|
coe1 = _mm_set1_epi16(1);
|
||||||
|
recp1 = _mm_rcp_ps(_mm_cvtepi32_ps(_mm_set1_epi32(length)));
|
||||||
|
|
||||||
|
//Acc
|
||||||
mm0 = _mm_setzero_si64();//pxor(mm0,mm0);
|
for (i = 0; i < (length >> 2); i++) {
|
||||||
mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
|
in = _mm_loadu_si128((__m128i *)input);
|
||||||
|
in_clp = _mm_subs_epi16(in, _mm_cmpeq_epi16(in, i16_min));//if in=SHRT_MIN in+1, else in
|
||||||
for (i=0; i<length>>1; i++) {
|
num0 = _mm_add_ps(num0, _mm_cvtepi32_ps(_mm_madd_epi16(in_clp, in_clp)));
|
||||||
|
num1 = _mm_add_ps(num1, _mm_cvtepi32_ps(_mm_madd_epi16(in, coe1)));//DC
|
||||||
mm1 = in[i];
|
input += 4;
|
||||||
mm2 = mm1;
|
|
||||||
mm1 = _m_pmaddwd(mm1,mm1);
|
|
||||||
mm1 = _m_psradi(mm1,shift);// shift any 32 bits blocs of the word by the value shift
|
|
||||||
mm0 = _m_paddd(mm0,mm1);// add the two 64 bits words 4 bytes by 4 bytes
|
|
||||||
// mm2 = _m_psrawi(mm2,shift_DC);
|
|
||||||
mm3 = _m_paddw(mm3,mm2);// add the two 64 bits words 2 bytes by 2 bytes
|
|
||||||
}
|
}
|
||||||
|
//Ave
|
||||||
|
num2 = _mm_dp_ps(num0, recp1, 0xFF);//AC power
|
||||||
|
num3 = _mm_dp_ps(num1, recp1, 0xFF);//DC
|
||||||
|
num3 = _mm_mul_ps(num3, num3); //DC power
|
||||||
|
//remove DC
|
||||||
|
temp = _mm_cvtsi128_si32(_mm_cvttps_epi32(_mm_sub_ps(num2, num3)));
|
||||||
|
|
||||||
mm1 = mm0;
|
return temp;
|
||||||
mm0 = _m_psrlqi(mm0,32);
|
|
||||||
mm0 = _m_paddd(mm0,mm1);
|
|
||||||
temp = _m_to_int(mm0);
|
|
||||||
temp/=length;
|
|
||||||
temp<<=shift; // this is the average of x^2
|
|
||||||
|
|
||||||
|
|
||||||
// now remove the DC component
|
|
||||||
|
|
||||||
|
|
||||||
mm2 = _m_psrlqi(mm3,32);
|
|
||||||
mm2 = _m_paddw(mm2,mm3);
|
|
||||||
mm2 = _m_pmaddwd(mm2,mm2);
|
|
||||||
temp2 = _m_to_int(mm2);
|
|
||||||
temp2/=(length*length);
|
|
||||||
// temp2<<=(2*shift_DC);
|
|
||||||
temp -= temp2;
|
|
||||||
|
|
||||||
_mm_empty();
|
|
||||||
_m_empty();
|
|
||||||
|
|
||||||
return((temp>0)?temp:1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int32_t signal_energy_amp_shift(int32_t *input,uint32_t length)
|
int32_t signal_energy_amp_shift(int32_t *input,uint32_t length)
|
||||||
@@ -164,60 +199,86 @@ int32_t signal_energy_amp_shift(int32_t *input,uint32_t length)
|
|||||||
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
|
int32_t signal_energy_nodc(int32_t *input,uint32_t length)
|
||||||
{
|
{
|
||||||
|
|
||||||
|
// int32_t i;
|
||||||
|
// int32_t temp;
|
||||||
|
// register __m64 mm0,mm1;//,mm2,mm3;
|
||||||
|
// __m64 *in = (__m64 *)input;
|
||||||
|
//
|
||||||
|
//#ifdef MAIN
|
||||||
|
// int16_t *printb;
|
||||||
|
//#endif
|
||||||
|
//
|
||||||
|
// mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
|
||||||
|
// // mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
|
||||||
|
//
|
||||||
|
// for (i=0; i<length>>1; i++) {
|
||||||
|
//
|
||||||
|
// mm1 = in[i];
|
||||||
|
// mm1 = _m_pmaddwd(mm1,mm1);// SIMD complex multiplication
|
||||||
|
// mm1 = _m_psradi(mm1,shift);
|
||||||
|
// mm0 = _m_paddd(mm0,mm1);
|
||||||
|
// // temp2 = mm0;
|
||||||
|
// // printf("%d %d\n",((int *)&in[i])[0],((int *)&in[i])[1]);
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// // printb = (int16_t *)&mm2;
|
||||||
|
// // printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// /*
|
||||||
|
// #ifdef MAIN
|
||||||
|
// printb = (int16_t *)&mm3;
|
||||||
|
// printf("%d %d %d %d\n",printb[0],printb[1],printb[2],printb[3]);
|
||||||
|
// #endif
|
||||||
|
// */
|
||||||
|
// mm1 = mm0;
|
||||||
|
//
|
||||||
|
// mm0 = _m_psrlqi(mm0,32);
|
||||||
|
//
|
||||||
|
// mm0 = _m_paddd(mm0,mm1);
|
||||||
|
//
|
||||||
|
// temp = _m_to_int(mm0);
|
||||||
|
//
|
||||||
|
// temp/=length;
|
||||||
|
// temp<<=shift; // this is the average of x^2
|
||||||
|
//
|
||||||
|
//#ifdef MAIN
|
||||||
|
// printf("E x^2 = %d\n",temp);
|
||||||
|
//#endif
|
||||||
|
// _mm_empty();
|
||||||
|
// _m_empty();
|
||||||
|
//
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// return((temp>0)?temp:1);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int32_t i;
|
int32_t i;
|
||||||
int32_t temp;
|
int32_t temp;
|
||||||
register __m64 mm0,mm1;//,mm2,mm3;
|
|
||||||
__m64 *in = (__m64 *)input;
|
|
||||||
|
|
||||||
#ifdef MAIN
|
|
||||||
int16_t *printb;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
mm0 = _mm_setzero_si64();//_pxor(mm0,mm0);
|
|
||||||
// mm3 = _mm_setzero_si64();//pxor(mm3,mm3);
|
|
||||||
|
|
||||||
for (i=0; i<length>>1; i++) {
|
|
||||||
|
|
||||||
mm1 = in[i];
|
|
||||||
mm1 = _m_pmaddwd(mm1,mm1);// SIMD complex multiplication
|
|
||||||
mm1 = _m_psradi(mm1,shift);
|
|
||||||
mm0 = _m_paddd(mm0,mm1);
|
|
||||||
// temp2 = mm0;
|
|
||||||
// printf("%d %d\n",((int *)&in[i])[0],((int *)&in[i])[1]);
|
|
||||||
|
|
||||||
|
|
||||||
// printb = (int16_t *)&mm2;
|
|
||||||
// printf("mm2 %d : %d %d %d %d\n",i,printb[0],printb[1],printb[2],printb[3]);
|
|
||||||
|
|
||||||
|
__m128i in;
|
||||||
|
__m128 mm0;
|
||||||
|
|
||||||
|
//init
|
||||||
|
mm0 = _mm_setzero_ps();
|
||||||
|
//Acc
|
||||||
|
for (i=0; i<(length>>2); i++) {
|
||||||
|
in = _mm_loadu_si128((__m128i *)input);
|
||||||
|
mm0 = _mm_add_ps(mm0,_mm_cvtepi32_ps(_mm_madd_epi16(in,in)));
|
||||||
|
input += 4;
|
||||||
}
|
}
|
||||||
|
//Ave
|
||||||
|
temp = (int)((((float*)&mm0)[0] +
|
||||||
|
((float*)&mm0)[1] +
|
||||||
|
((float*)&mm0)[2] +
|
||||||
|
((float*)&mm0)[3])/(float)length);
|
||||||
|
|
||||||
/*
|
return temp;
|
||||||
#ifdef MAIN
|
|
||||||
printb = (int16_t *)&mm3;
|
|
||||||
printf("%d %d %d %d\n",printb[0],printb[1],printb[2],printb[3]);
|
|
||||||
#endif
|
|
||||||
*/
|
|
||||||
mm1 = mm0;
|
|
||||||
|
|
||||||
mm0 = _m_psrlqi(mm0,32);
|
|
||||||
|
|
||||||
mm0 = _m_paddd(mm0,mm1);
|
|
||||||
|
|
||||||
temp = _m_to_int(mm0);
|
|
||||||
|
|
||||||
temp/=length;
|
|
||||||
temp<<=shift; // this is the average of x^2
|
|
||||||
|
|
||||||
#ifdef MAIN
|
|
||||||
printf("E x^2 = %d\n",temp);
|
|
||||||
#endif
|
|
||||||
_mm_empty();
|
|
||||||
_m_empty();
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
return((temp>0)?temp:1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#elif defined(__arm__)
|
#elif defined(__arm__)
|
||||||
|
|||||||
Reference in New Issue
Block a user