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_DC 0
|
||||
#define SHRT_MIN -32768
|
||||
|
||||
#if defined(__x86_64__) || defined(__i386__)
|
||||
#ifdef LOCALIZATION
|
||||
@@ -67,52 +68,86 @@ int32_t subcarrier_energy(int32_t *input,uint32_t length, int32_t *subcarrier_en
|
||||
}
|
||||
#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)
|
||||
{
|
||||
uint32_t i;
|
||||
int32_t temp;
|
||||
__m128i in, in_clp, i16_min, coe1;
|
||||
__m128 num0, num1, num2, num3, recp1;
|
||||
|
||||
int32_t i;
|
||||
int32_t temp,temp2;
|
||||
register __m64 mm0,mm1,mm2,mm3;
|
||||
__m64 *in = (__m64 *)input;
|
||||
//init
|
||||
num0 = _mm_setzero_ps();
|
||||
num1 = _mm_setzero_ps();
|
||||
i16_min = _mm_set1_epi16(SHRT_MIN);
|
||||
coe1 = _mm_set1_epi16(1);
|
||||
recp1 = _mm_rcp_ps(_mm_cvtepi32_ps(_mm_set1_epi32(length)));
|
||||
|
||||
|
||||
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
|
||||
//Acc
|
||||
for (i = 0; i < (length >> 2); i++) {
|
||||
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
|
||||
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
|
||||
input += 4;
|
||||
}
|
||||
//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;
|
||||
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
|
||||
return temp;
|
||||
|
||||
|
||||
// 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)
|
||||
@@ -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 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 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);
|
||||
|
||||
/*
|
||||
#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;
|
||||
|
||||
|
||||
|
||||
return((temp>0)?temp:1);
|
||||
}
|
||||
|
||||
#elif defined(__arm__)
|
||||
|
||||
Reference in New Issue
Block a user