restore_bplatform_bin_file ()
{
int rr;
/* 0 */
read_fm( rr, 1 );
read_fm(vnbs,1);
read_fm(ttms,1);
read_fm(vad(1),vnbs+1);
restore_alloc ( vad(vnbs+1) );
read_fm(mmts[1],vad(vnbs+1));
read_fm(vtt(1),vnbs);
read_fm(tmbs[1],ttms);
mtt(vnbs+1,0); /* !!! */
mad(vnbs+2,max_mem); /* !!! */
/* 1 */
read_fm( rr, 1 );
read_fm(sym,1);
read_fm(stri[1],sym);
read_fm(lexi[1],sym);
read_fm(edit[1],sym);
read_fm(prio[1],sym);
read_fm(righ[1],sym);
read_fm(clos[1],sym);
read_fm(arity[1],sym);
/* 2 */
read_fm( rr, 1 );
read_fm(hcod[1],max_sym);
/* 3 */
read_fm( rr, 1 );
read_fm(rul,1);
read_fm(ant[1],rul);
read_fm(cns[1],rul);
read_fm(rth[1],rul);
read_fm(num[1],rul);
read_fm(trl[1],rul);
read_fm(lsb[1],rul);
read_fm(pvd[1],rul);
read_fm(sts[1],rul);
/* 4 */
read_fm( rr, 1 );
read_fm(rttm,1);
read_fm(rtmb[1],rttm);
/* 7 */
read_fm( rr, 1 );
read_fm(the,1);
read_fm(tru[1],the);
read_fm(tna[1],the);
read_fm(tnm[1],the);
read_fm(ttc[1],the);
read_fm(tft[1],the);
/* 8 */
read_fm( rr, 1 );
read_fm(tttm,1);
read_fm(ttmb[1],tttm);
/* 9 */
read_fm( rr, 1 );
read_fm(ttdi,1);
read_fm(tdir[1],ttdi);
}
void Coder_ld8a(
Word16 ana[], /* output : Analysis parameters */
Word16 frame, /* input : frame counter */
Word16 vad_enable /* input : VAD enable flag */
)
{
/* LPC analysis */
Word16 Aq_t[(MP1)*2]; /* A(z) quantized for the 2 subframes */
Word16 Ap_t[(MP1)*2]; /* A(z/gamma) for the 2 subframes */
Word16 *Aq, *Ap; /* Pointer on Aq_t and Ap_t */
/* Other vectors */
Word16 h1[L_SUBFR]; /* Impulse response h1[] */
Word16 xn[L_SUBFR]; /* Target vector for pitch search */
Word16 xn2[L_SUBFR]; /* Target vector for codebook search */
Word16 code[L_SUBFR]; /* Fixed codebook excitation */
Word16 y1[L_SUBFR]; /* Filtered adaptive excitation */
Word16 y2[L_SUBFR]; /* Filtered fixed codebook excitation */
Word16 g_coeff[4]; /* Correlations between xn & y1 */
Word16 g_coeff_cs[5];
Word16 exp_g_coeff_cs[5]; /* Correlations between xn, y1, & y2
<y1,y1>, -2<xn,y1>,
<y2,y2>, -2<xn,y2>, 2<y1,y2> */
/* Scalars */
Word16 i, j, k, i_subfr;
Word16 T_op, T0, T0_min, T0_max, T0_frac;
Word16 gain_pit, gain_code, index;
Word16 temp, taming;
Word32 L_temp;
/*------------------------------------------------------------------------*
* - Perform LPC analysis: *
* * autocorrelation + lag windowing *
* * Levinson-durbin algorithm to find a[] *
* * convert a[] to lsp[] *
* * quantize and code the LSPs *
* * find the interpolated LSPs and convert to a[] for the 2 *
* subframes (both quantized and unquantized) *
*------------------------------------------------------------------------*/
{
/* Temporary vectors */
Word16 r_l[NP+1], r_h[NP+1]; /* Autocorrelations low and hi */
Word16 rc[M]; /* Reflection coefficients. */
Word16 lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe */
/* For G.729B */
Word16 rh_nbe[MP1];
Word16 lsf_new[M];
Word16 lsfq_mem[MA_NP][M];
Word16 exp_R0, Vad;
/* LP analysis */
Autocorr(p_window, NP, r_h, r_l, &exp_R0); /* Autocorrelations */
Copy(r_h, rh_nbe, MP1);
Lag_window(NP, r_h, r_l); /* Lag windowing */
Levinson(r_h, r_l, Ap_t, rc, &temp); /* Levinson Durbin */
Az_lsp(Ap_t, lsp_new, lsp_old); /* From A(z) to lsp */
/* For G.729B */
/* ------ VAD ------- */
Lsp_lsf(lsp_new, lsf_new, M);
vad(rc[1], lsf_new, r_h, r_l, exp_R0, p_window, frame,
pastVad, ppastVad, &Vad);
Update_cng(rh_nbe, exp_R0, Vad);
/* ---------------------- */
/* Case of Inactive frame */
/* ---------------------- */
if ((Vad == 0) && (vad_enable == 1)){
Get_freq_prev(lsfq_mem);
Cod_cng(exc, pastVad, lsp_old_q, Aq_t, ana, lsfq_mem, &seed);
Update_freq_prev(lsfq_mem);
ppastVad = pastVad;
pastVad = Vad;
/* Update wsp, mem_w and mem_w0 */
Aq = Aq_t;
for(i_subfr=0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
/* Residual signal in xn */
Residu(Aq, &speech[i_subfr], xn, L_SUBFR);
Weight_Az(Aq, GAMMA1, M, Ap_t);
/* Compute wsp and mem_w */
Ap = Ap_t + MP1;
Ap[0] = 4096;
for(i=1; i<=M; i++) /* Ap[i] = Ap_t[i] - 0.7 * Ap_t[i-1]; */
Ap[i] = sub(Ap_t[i], mult(Ap_t[i-1], 22938));
Syn_filt(Ap, xn, &wsp[i_subfr], L_SUBFR, mem_w, 1);
/* Compute mem_w0 */
for(i=0; i<L_SUBFR; i++) {
xn[i] = sub(xn[i], exc[i_subfr+i]); /* residu[] - exc[] */
}
Syn_filt(Ap_t, xn, xn, L_SUBFR, mem_w0, 1);
Aq += MP1;
}
sharp = SHARPMIN;
/* Update memories for next frames */
Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
return;
} /* End of inactive frame case */
/* -------------------- */
/* Case of Active frame */
/* -------------------- */
/* Case of active frame */
*ana++ = 1;
seed = INIT_SEED;
ppastVad = pastVad;
pastVad = Vad;
/* LSP quantization */
Qua_lsp(lsp_new, lsp_new_q, ana);
ana += 2; /* Advance analysis parameters pointer */
/*--------------------------------------------------------------------*
* Find interpolated LPC parameters in all subframes *
* The interpolated parameters are in array Aq_t[]. *
*--------------------------------------------------------------------*/
Int_qlpc(lsp_old_q, lsp_new_q, Aq_t);
/* Compute A(z/gamma) */
Weight_Az(&Aq_t[0], GAMMA1, M, &Ap_t[0]);
Weight_Az(&Aq_t[MP1], GAMMA1, M, &Ap_t[MP1]);
/* update the LSPs for the next frame */
Copy(lsp_new, lsp_old, M);
Copy(lsp_new_q, lsp_old_q, M);
}
/*----------------------------------------------------------------------*
* - Find the weighted input speech w_sp[] for the whole speech frame *
* - Find the open-loop pitch delay *
*----------------------------------------------------------------------*/
Residu(&Aq_t[0], &speech[0], &exc[0], L_SUBFR);
Residu(&Aq_t[MP1], &speech[L_SUBFR], &exc[L_SUBFR], L_SUBFR);
{
Word16 Ap1[MP1];
Ap = Ap_t;
Ap1[0] = 4096;
for(i=1; i<=M; i++) /* Ap1[i] = Ap[i] - 0.7 * Ap[i-1]; */
Ap1[i] = sub(Ap[i], mult(Ap[i-1], 22938));
Syn_filt(Ap1, &exc[0], &wsp[0], L_SUBFR, mem_w, 1);
Ap += MP1;
for(i=1; i<=M; i++) /* Ap1[i] = Ap[i] - 0.7 * Ap[i-1]; */
Ap1[i] = sub(Ap[i], mult(Ap[i-1], 22938));
Syn_filt(Ap1, &exc[L_SUBFR], &wsp[L_SUBFR], L_SUBFR, mem_w, 1);
}
/* Find open loop pitch lag */
T_op = Pitch_ol_fast(wsp, PIT_MAX, L_FRAME);
/* Range for closed loop pitch search in 1st subframe */
T0_min = sub(T_op, 3);
if (sub(T0_min,PIT_MIN)<0) {
T0_min = PIT_MIN;
}
T0_max = add(T0_min, 6);
if (sub(T0_max ,PIT_MAX)>0)
{
T0_max = PIT_MAX;
T0_min = sub(T0_max, 6);
}
/*------------------------------------------------------------------------*
* Loop for every subframe in the analysis frame *
*------------------------------------------------------------------------*
* To find the pitch and innovation parameters. The subframe size is *
* L_SUBFR and the loop is repeated 2 times. *
* - find the weighted LPC coefficients *
* - find the LPC residual signal res[] *
* - compute the target signal for pitch search *
* - compute impulse response of weighted synthesis filter (h1[]) *
* - find the closed-loop pitch parameters *
* - encode the pitch delay *
* - find target vector for codebook search *
* - codebook search *
* - VQ of pitch and codebook gains *
* - update states of weighting filter *
*------------------------------------------------------------------------*/
Aq = Aq_t; /* pointer to interpolated quantized LPC parameters */
Ap = Ap_t; /* pointer to weighted LPC coefficients */
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
/*---------------------------------------------------------------*
* Compute impulse response, h1[], of weighted synthesis filter *
*---------------------------------------------------------------*/
h1[0] = 4096;
Set_zero(&h1[1], L_SUBFR-1);
Syn_filt(Ap, h1, h1, L_SUBFR, &h1[1], 0);
/*----------------------------------------------------------------------*
* Find the target vector for pitch search: *
*----------------------------------------------------------------------*/
Syn_filt(Ap, &exc[i_subfr], xn, L_SUBFR, mem_w0, 0);
/*---------------------------------------------------------------------*
* Closed-loop fractional pitch search *
*---------------------------------------------------------------------*/
T0 = Pitch_fr3_fast(&exc[i_subfr], xn, h1, L_SUBFR, T0_min, T0_max,
i_subfr, &T0_frac);
index = Enc_lag3(T0, T0_frac, &T0_min, &T0_max,PIT_MIN,PIT_MAX,i_subfr);
*ana++ = index;
if (i_subfr == 0) {
*ana++ = Parity_Pitch(index);
}
/*-----------------------------------------------------------------*
* - find filtered pitch exc *
* - compute pitch gain and limit between 0 and 1.2 *
* - update target vector for codebook search *
*-----------------------------------------------------------------*/
Syn_filt(Ap, &exc[i_subfr], y1, L_SUBFR, mem_zero, 0);
gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR);
/* clip pitch gain if taming is necessary */
taming = test_err(T0, T0_frac);
if( taming == 1){
if (sub(gain_pit, GPCLIP) > 0) {
gain_pit = GPCLIP;
}
}
/* xn2[i] = xn[i] - y1[i] * gain_pit */
for (i = 0; i < L_SUBFR; i++)
{
L_temp = L_mult(y1[i], gain_pit);
L_temp = L_shl(L_temp, 1); /* gain_pit in Q14 */
xn2[i] = sub(xn[i], extract_h(L_temp));
}
/*-----------------------------------------------------*
* - Innovative codebook search. *
*-----------------------------------------------------*/
index = ACELP_Code_A(xn2, h1, T0, sharp, code, y2, &i);
*ana++ = index; /* Positions index */
*ana++ = i; /* Signs index */
/*-----------------------------------------------------*
* - Quantization of gains. *
*-----------------------------------------------------*/
g_coeff_cs[0] = g_coeff[0]; /* <y1,y1> */
exp_g_coeff_cs[0] = negate(g_coeff[1]); /* Q-Format:XXX -> JPN */
g_coeff_cs[1] = negate(g_coeff[2]); /* (xn,y1) -> -2<xn,y1> */
exp_g_coeff_cs[1] = negate(add(g_coeff[3], 1)); /* Q-Format:XXX -> JPN */
Corr_xy2( xn, y1, y2, g_coeff_cs, exp_g_coeff_cs ); /* Q0 Q0 Q12 ^Qx ^Q0 */
/* g_coeff_cs[3]:exp_g_coeff_cs[3] = <y2,y2> */
/* g_coeff_cs[4]:exp_g_coeff_cs[4] = -2<xn,y2> */
/* g_coeff_cs[5]:exp_g_coeff_cs[5] = 2<y1,y2> */
*ana++ = Qua_gain(code, g_coeff_cs, exp_g_coeff_cs,
L_SUBFR, &gain_pit, &gain_code, taming);
/*------------------------------------------------------------*
* - Update pitch sharpening "sharp" with quantized gain_pit *
*------------------------------------------------------------*/
sharp = gain_pit;
if (sub(sharp, SHARPMAX) > 0) { sharp = SHARPMAX; }
if (sub(sharp, SHARPMIN) < 0) { sharp = SHARPMIN; }
/*------------------------------------------------------*
* - Find the total excitation *
* - update filters memories for finding the target *
* vector in the next subframe *
*------------------------------------------------------*/
for (i = 0; i < L_SUBFR; i++)
{
/* exc[i] = gain_pit*exc[i] + gain_code*code[i]; */
/* exc[i] in Q0 gain_pit in Q14 */
/* code[i] in Q13 gain_cod in Q1 */
L_temp = L_mult(exc[i+i_subfr], gain_pit);
L_temp = L_mac(L_temp, code[i], gain_code);
L_temp = L_shl(L_temp, 1);
exc[i+i_subfr] = round(L_temp);
}
update_exc_err(gain_pit, T0);
for (i = L_SUBFR-M, j = 0; i < L_SUBFR; i++, j++)
{
temp = extract_h(L_shl( L_mult(y1[i], gain_pit), 1) );
k = extract_h(L_shl( L_mult(y2[i], gain_code), 2) );
mem_w0[j] = sub(xn[i], add(temp, k));
}
Aq += MP1; /* interpolated LPC parameters for next subframe */
Ap += MP1;
}
/*--------------------------------------------------*
* Update signal for next frame. *
* -> shift to the left by L_FRAME: *
* speech[], wsp[] and exc[] *
*--------------------------------------------------*/
Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
return;
}
save_bplatform_bin_file ()
{
int rr;
srand ( ( int ) ( ( time ( NULL ) % 65536 ) ) );
rr = ( int ) rand ();
/* 0 */
write_fm ( rr, 1 );
write_fm(vnbs,1);
write_fm(ttms,1);
write_fm(vad(1),vnbs+1);
write_fm(mmts[1],vad(vnbs+1));
write_fm(vtt(1),vnbs);
write_fm(tmbs[1],ttms);
/* 1 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(sym,1);
write_fm(stri[1],sym);
write_fm(lexi[1],sym);
write_fm(edit[1],sym);
write_fm(prio[1],sym);
write_fm(righ[1],sym);
write_fm(clos[1],sym);
write_fm(arity[1],sym);
/* 2 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(hcod[1],max_sym);
/* 3 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(rul,1);
write_fm(ant[1],rul);
write_fm(cns[1],rul);
write_fm(rth[1],rul);
write_fm(num[1],rul);
write_fm(trl[1],rul);
write_fm(lsb[1],rul);
write_fm(pvd[1],rul);
write_fm(sts[1],rul);
/* 4 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(rttm,1);
write_fm(rtmb[1],rttm);
/* 7 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(the,1);
write_fm(tru[1],the);
write_fm(tna[1],the);
write_fm(tnm[1],the);
write_fm(ttc[1],the);
write_fm(tft[1],the);
/* 8 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(tttm,1);
write_fm(ttmb[1],tttm);
/* 9 */
rr = ( int ) rand ();
write_fm ( rr, 1 );
write_fm(ttdi,1);
write_fm(tdir[1],ttdi);
}
int main(void){
float Es,E;
unsigned int i, j;
unsigned int doa_aux[3];
int diff[3];
#ifdef _FLASH
memcpy(&RamfuncsRunStart, &RamfuncsLoadStart, (size_t)&RamfuncsLoadSize);
#endif
/*------------------------------------------------------*/
/* Inicialización */
/*------------------------------------------------------*/
InitSysCtrl();
InitSysPll(XTAL_OSC,IMULT_20,FMULT_0,PLLCLK_BY_2);
EDIS;
InitGpio();
InitPieCtrl();
IER = 0x0000;
IFR = 0x0000;
InitPieVectTable();
//Configura operación del ADC-A
ADC_Configure(ADCA,16000);
//Configura los canales 2,3,4 y 5 del ADC-A
ADC_Init(ADCA, 2);
ADC_Init(ADCA, 3);
ADC_Init(ADCA, 4);
//La interrupción del ADC-A se da cuando termine el canal 4
ADC_Int(ADCA, 4);
hnd_cfft->OutPtr = CFFToutBuff; // Apuntador al Buffer de salida
hnd_cfft->Stages = STAGE; // Número de etapas de la FFT
hnd_cfft->FFTSize = N; // Tamaño de la FFT
hnd_cfft->CoefPtr = CFFTF32Coef; // Auntador a los coeficientes de Fourier
CFFT_f32_sincostable(hnd_cfft); // Calcula los factores de Fourier
//Configura el puerto serial
Serial_Init();
Serial_Configure(BR9600);
Serial_Start();
ADC_Start(ADCA); //Inicia la conversión del ADC-A
cont = 0;
//calculo de la energía del silencio (que filosófico suena esto)
EINT;
Es = 0;
while(cont<(N<<1));
ServiceDog();
cont = 0;
Es = energy(x);
while(cont<(N<<1));
ServiceDog();
cont = 0;
Es += energy(x);
Es = Es/2;
doaG = 0;
while(1){
#ifndef DEBUG
init = false;
while(!init);
#endif
E = Es;
for(j=0;j<3;j++){
//recibe datos y verifica si es ruido o no
do{
ADC_Start(ADCA);
while(cont<(N<<1));
cont = 0; //Una vez llenos los buffers de datos procedemos a realizar el algoritmo
ADC_Stop(ADCA); //detiene la adquisición para obtener las FFT
ServiceDog();
}while(!vad(x,E));
//FFT mic 1
hnd_cfft->InPtr = x1;
CFFT_f32u(hnd_cfft);
for(i=0;i<(N<<1);i++){
xw1[i] = hnd_cfft->CurrentInPtr[i];
}
//FFT mic 2
hnd_cfft->InPtr = x2;
CFFT_f32u(hnd_cfft);
for(i=0;i<(N<<1);i++){
xw2[i] = hnd_cfft->CurrentInPtr[i];
}
//FFT mic 3
hnd_cfft->InPtr = x3;
CFFT_f32u(hnd_cfft);
for(i=0;i<(N<<1);i++){
xw3[i] = hnd_cfft->CurrentInPtr[i];
}
ServiceDog();
doa_aux[j] = doa_est(xw1,xw2,xw3,30); //50
//doaG +=doa_aux[j];
DELAY_US(100000); //retraso de 100ms
ServiceDog();
E = 0.8*E;
}//for j
diff[0] = doa_aux[0]-doa_aux[1]; //diferencia entre primer y segundo frame
diff[1] = doa_aux[1]-doa_aux[2]; //diferencia entre segundo y tercer frame
diff[2] = doa_aux[0]-doa_aux[2]; //diferencia entre primer y tercer frame
if(diff[0]<0)
diff[0] = -diff[0];
if(diff[1]<0)
diff[1] = -diff[1];
if(diff[2]<0)
diff[2] = -diff[2];
if( diff[0]<=diff[1] && diff[0]<=diff[2] )
doaG = (doa_aux[0]+doa_aux[1])>>1;
else if ( diff[1]<=diff[0] && diff[1]<=diff[2] )
doaG = (doa_aux[1]+doa_aux[2])>>1;
else
