static Word16
MR795_gain_code_quant_mod( /* o : index of quantization. */
Word16 gain_pit, /* i : pitch gain, Q14 */
Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */
Word16 gcode0, /* i : predicted CB gain (norm.), Q14 */
Word16 frac_en[], /* i : energy coefficients (4),
fraction part, Q15 */
Word16 exp_en[], /* i : energy coefficients (4),
eponent part, Q0 */
Word16 alpha, /* i : gain adaptor factor (>0), Q15 */
Word16 gain_cod_unq, /* i : Code gain (unquantized) */
/* (scaling: Q10 - exp_gcode0) */
Word16 *gain_cod, /* i/o: Code gain (pre-/quantized), Q1 */
Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */
/* (for MR122 MA predictor update) */
Word16 *qua_ener, /* o : quantized energy error, Q10 */
/* (for other MA predictor update) */
const Word16* qua_gain_code_ptr, /* i : ptr to read-only ptr */
Flag *pOverflow /* o : overflow indicator */
)
{
const Word16 *p;
Word16 i;
Word16 index;
Word16 tmp;
Word16 one_alpha;
Word16 exp;
Word16 e_max;
Word16 g2_pitch;
Word16 g_code;
Word16 g2_code_h;
Word16 g2_code_l;
Word16 d2_code_h;
Word16 d2_code_l;
Word16 coeff[5];
Word16 coeff_lo[5];
Word16 exp_coeff[5];
Word32 L_tmp;
Word32 L_t0;
Word32 L_t1;
Word32 dist_min;
Word16 gain_code;
/*
Steps in calculation of the error criterion (dist):
---------------------------------------------------
underlined = constant; alp = FLP value of alpha, alpha = FIP
----------
ExEn = gp^2 * LtpEn + 2.0*gp*gc[i] * XC + gc[i]^2 * InnEn;
------------ ------ -- -----
aExEn= alp * ExEn
= alp*gp^2*LtpEn + 2.0*alp*gp*XC* gc[i] + alp*InnEn* gc[i]^2
-------------- ------------- ---------
= t[1] + t[2] + t[3]
dist = d1 + d2;
d1 = (1.0 - alp) * InnEn * (gcu - gc[i])^2 = t[4]
------------------- ---
d2 = alp * (ResEn - 2.0 * sqrt(ResEn*ExEn) + ExEn);
--- ----- --- -----
= alp * (sqrt(ExEn) - sqrt(ResEn))^2
--- -----------
= (sqrt(aExEn) - sqrt(alp*ResEn))^2
---------------
= (sqrt(aExEn) - t[0] )^2
----
*/
/*
* calculate scalings of the constant terms
*/
gain_code = shl(*gain_cod, (10 - exp_gcode0), pOverflow); /* Q1 -> Q11 (-ec0) */
g2_pitch = mult(gain_pit, gain_pit, pOverflow); /* Q14 -> Q13 */
/* 0 < alpha <= 0.5 => 0.5 <= 1-alpha < 1, i.e one_alpha is normalized */
one_alpha = add_16((32767 - alpha), 1, pOverflow); /* 32768 - alpha */
/* alpha <= 0.5 -> mult. by 2 to keep precision; compensate in exponent */
L_t1 = L_mult(alpha, frac_en[1], pOverflow);
L_t1 = L_shl(L_t1, 1, pOverflow);
tmp = (Word16)(L_t1 >> 16);
/* directly store in 32 bit variable because no further mult. required */
L_t1 = L_mult(tmp, g2_pitch, pOverflow);
exp_coeff[1] = exp_en[1] - 15;
tmp = (Word16)(L_shl(L_mult(alpha, frac_en[2], pOverflow), 1, pOverflow) >> 16);
coeff[2] = mult(tmp, gain_pit, pOverflow);
exp = exp_gcode0 - 10;
exp_coeff[2] = add_16(exp_en[2], exp, pOverflow);
/* alpha <= 0.5 -> mult. by 2 to keep precision; compensate in exponent */
coeff[3] = (Word16)(L_shl(L_mult(alpha, frac_en[3], pOverflow), 1, pOverflow) >> 16);
exp = shl(exp_gcode0, 1, pOverflow) - 7;
exp_coeff[3] = add_16(exp_en[3], exp, pOverflow);
coeff[4] = mult(one_alpha, frac_en[3], pOverflow);
exp_coeff[4] = add_16(exp_coeff[3], 1, pOverflow);
L_tmp = L_mult(alpha, frac_en[0], pOverflow);
/* sqrt_l returns normalized value and 2*exponent
-> result = val >> (exp/2)
exp_coeff holds 2*exponent for c[0] */
/* directly store in 32 bit variable because no further mult. required */
L_t0 = sqrt_l_exp(L_tmp, &exp, pOverflow); /* normalization included in sqrt_l_exp */
exp += 47;
exp_coeff[0] = exp_en[0] - exp;
/*
* Determine the maximum exponent occuring in the distance calculation
* and adjust all fractions accordingly (including a safety margin)
*
*/
/* find max(e[1..4],e[0]+31) */
e_max = exp_coeff[0] + 31;
for (i = 1; i <= 4; i++)
{
if (exp_coeff[i] > e_max)
{
e_max = exp_coeff[i];
}
}
/* scale c[1] (requires no further multiplication) */
tmp = e_max - exp_coeff[1];
L_t1 = L_shr(L_t1, tmp, pOverflow);
/* scale c[2..4] (used in Mpy_32_16 in the quantizer loop) */
for (i = 2; i <= 4; i++)
{
tmp = e_max - exp_coeff[i];
L_tmp = ((Word32)coeff[i] << 16);
L_tmp = L_shr(L_tmp, tmp, pOverflow);
L_Extract(L_tmp, &coeff[i], &coeff_lo[i], pOverflow);
}
/* scale c[0] (requires no further multiplication) */
exp = e_max - 31; /* new exponent */
tmp = exp - exp_coeff[0];
L_t0 = L_shr(L_t0, shr(tmp, 1, pOverflow), pOverflow);
/* perform correction by 1/sqrt(2) if exponent difference is odd */
if ((tmp & 0x1) != 0)
{
L_Extract(L_t0, &coeff[0], &coeff_lo[0], pOverflow);
L_t0 = Mpy_32_16(coeff[0], coeff_lo[0],
23170, pOverflow); /* 23170 Q15 = 1/sqrt(2)*/
}
/* search the quantizer table for the lowest value
of the search criterion */
dist_min = MAX_32;
index = 0;
p = &qua_gain_code_ptr[0];
for (i = 0; i < NB_QUA_CODE; i++)
{
g_code = *p++; /* this is g_fac (Q11) */
p++; /* skip log2(g_fac) */
p++; /* skip 20*log10(g_fac) */
g_code = mult(g_code, gcode0, pOverflow);
/* only continue if gc[i] < 2.0*gc
which is equiv. to g_code (Q10-ec0) < gain_code (Q11-ec0) */
if (g_code >= gain_code)
{
break;
}
L_tmp = L_mult(g_code, g_code, pOverflow);
L_Extract(L_tmp, &g2_code_h, &g2_code_l, pOverflow);
tmp = sub(g_code, gain_cod_unq, pOverflow);
L_tmp = L_mult(tmp, tmp, pOverflow);
L_Extract(L_tmp, &d2_code_h, &d2_code_l, pOverflow);
/* t2, t3, t4 */
L_tmp = Mac_32_16(L_t1, coeff[2], coeff_lo[2], g_code, pOverflow);
L_tmp = Mac_32(L_tmp, coeff[3], coeff_lo[3], g2_code_h, g2_code_l, pOverflow);
L_tmp = sqrt_l_exp(L_tmp, &exp, pOverflow);
L_tmp = L_shr(L_tmp, shr(exp, 1, pOverflow), pOverflow);
/* d2 */
tmp = pv_round(L_sub(L_tmp, L_t0, pOverflow), pOverflow);
L_tmp = L_mult(tmp, tmp, pOverflow);
/* dist */
L_tmp = Mac_32(L_tmp, coeff[4], coeff_lo[4], d2_code_h, d2_code_l, pOverflow);
/* store table index if distance measure for this
index is lower than the minimum seen so far */
if (L_tmp < dist_min)
{
dist_min = L_tmp;
index = i;
}
}
/*------------------------------------------------------------------*
* read quantized gains and new values for MA predictor memories *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
*------------------------------------------------------------------*/
/* Read the quantized gains */
p = &qua_gain_code_ptr[(index<<2) - index];
g_code = *p++;
*qua_ener_MR122 = *p++;
*qua_ener = *p;
/*------------------------------------------------------------------*
* calculate final fixed codebook gain: *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* gc = gc0 * g *
*------------------------------------------------------------------*/
L_tmp = L_mult(g_code, gcode0, pOverflow);
L_tmp = L_shr(L_tmp, 9 - exp_gcode0, pOverflow);
*gain_cod = (Word16)(L_tmp >> 16);
return index;
}
/*---------------------------------------------------------------------------*
* Function Dec_gain *
* ~~~~~~~~~~~~~~~~~~ *
* Decode the pitch and codebook gains *
* *
*---------------------------------------------------------------------------*
* input arguments: *
* *
* index :Quantization index *
* code[] :Innovative code vector *
* L_subfr :Subframe size *
* bfi :Bad frame indicator *
* *
* output arguments: *
* *
* gain_pit :Quantized pitch gain *
* gain_cod :Quantized codebook gain *
* *
*---------------------------------------------------------------------------*/
void WebRtcG729fix_Dec_gain(
Decod_ld8a_state *st,
int16_t index, /* (i) :Index of quantization. */
int16_t code[], /* (i) Q13 :Innovative vector. */
int16_t L_subfr, /* (i) :Subframe length. */
int16_t bfi, /* (i) :Bad frame indicator */
int16_t *gain_pit, /* (o) Q14 :Pitch gain. */
int16_t *gain_cod /* (o) Q1 :Code gain. */
)
{
int16_t index1, index2, tmp;
int16_t gcode0, exp_gcode0;
int32_t L_gbk12, L_acc, L_accb;
/*-------------- Case of erasure. ---------------*/
if(bfi != 0){
*gain_pit = mult( *gain_pit, 29491 ); /* *0.9 in Q15 */
if (*gain_pit > 29491) *gain_pit = 29491;
*gain_cod = mult( *gain_cod, 32111 ); /* *0.98 in Q15 */
/*----------------------------------------------*
* update table of past quantized energies *
* (frame erasure) *
*----------------------------------------------*/
WebRtcG729fix_Gain_update_erasure(st->past_qua_en);
return;
}
/*-------------- Decode pitch gain ---------------*/
index1 = WebRtcG729fix_imap1[ shr(index,NCODE2_B) ] ;
index2 = WebRtcG729fix_imap2[ index & (NCODE2-1) ] ;
*gain_pit = WebRtcSpl_AddSatW16( WebRtcG729fix_gbk1[index1][0], WebRtcG729fix_gbk2[index2][0] );
/*-------------- Decode codebook gain ---------------*/
/*---------------------------------------------------*
*- energy due to innovation -*
*- predicted energy -*
*- predicted codebook gain => gcode0[exp_gcode0] -*
*---------------------------------------------------*/
WebRtcG729fix_Gain_predict(st->past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 );
/*-----------------------------------------------------------------*
* *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0; *
*-----------------------------------------------------------------*/
L_acc = L_deposit_l( WebRtcG729fix_gbk1[index1][1] );
L_accb = L_deposit_l( WebRtcG729fix_gbk2[index2][1] );
L_gbk12 = WebRtcSpl_AddSatW32( L_acc, L_accb ); /* Q13 */
tmp = extract_l( L_shr( L_gbk12,1 ) ); /* Q12 */
L_acc = L_mult(tmp, gcode0); /* Q[exp_gcode0+12+1] */
L_acc = L_shl(L_acc, WebRtcSpl_AddSatW16( negate(exp_gcode0),(-12-1+1+16) ));
*gain_cod = extract_h( L_acc ); /* Q1 */
/*----------------------------------------------*
* update table of past quantized energies *
*----------------------------------------------*/
WebRtcG729fix_Gain_update(st->past_qua_en, L_gbk12 );
return;
}
void perc_var (
Word16 *gamma1, /* Bandwidth expansion parameter */
Word16 *gamma2, /* Bandwidth expansion parameter */
Word16 *LsfInt, /* Interpolated LSP vector : 1st subframe */
Word16 *LsfNew, /* New LSP vector : 2nd subframe */
Word16 *r_c /* Reflection coefficients */
)
{
Word32 L_temp;
Word16 cur_rc; /* Q11 */
Word16 Lar[4]; /* Q11 */
Word16 *LarNew; /* Q11 */
Word16 *Lsf; /* Q15 */
Word16 CritLar0, CritLar1; /* Q11 */
Word16 temp;
Word16 d_min; /* Q10 */
Word16 i, k;
for (k=0; k<M; k++) {
LsfInt[k] = shl(LsfInt[k], 1);
LsfNew[k] = shl(LsfNew[k], 1);
}
LarNew = &Lar[2];
/* ---------------------------------------- */
/* Reflection coefficients ---> Lar */
/* Lar(i) = log10( (1+rc) / (1-rc) ) */
/* Approximated by */
/* x <= SEG1 y = x */
/* SEG1 < x <= SEG2 y = A1 x - B1_L */
/* SEG2 < x <= SEG3 y = A2 x - B2_L */
/* x > SEG3 y = A3 x - B3_L */
/* ---------------------------------------- */
for (i=0; i<2; i++) {
cur_rc = abs_s(r_c[i]);
cur_rc = shr(cur_rc, 4);
if (sub(cur_rc ,SEG1)<= 0) {
LarNew[i] = cur_rc;
}
else {
if (sub(cur_rc,SEG2)<= 0) {
cur_rc = shr(cur_rc, 1);
L_temp = L_mult(cur_rc, A1);
L_temp = L_sub(L_temp, L_B1);
L_temp = L_shr(L_temp, 11);
LarNew[i] = extract_l(L_temp);
}
else {
if (sub(cur_rc ,SEG3)<= 0) {
cur_rc = shr(cur_rc, 1);
L_temp = L_mult(cur_rc, A2);
L_temp = L_sub(L_temp, L_B2);
L_temp = L_shr(L_temp, 11);
LarNew[i] = extract_l(L_temp);
}
else {
cur_rc = shr(cur_rc, 1);
L_temp = L_mult(cur_rc, A3);
L_temp = L_sub(L_temp, L_B3);
L_temp = L_shr(L_temp, 11);
LarNew[i] = extract_l(L_temp);
}
}
}
if (r_c[i] < 0) {
LarNew[i] = sub(0, LarNew[i]);
}
}
/* Interpolation of Lar for the 1st subframe */
temp = add(LarNew[0], LarOld[0]);
Lar[0] = shr(temp, 1);
LarOld[0] = LarNew[0];
temp = add(LarNew[1], LarOld[1]);
Lar[1] = shr(temp, 1);
LarOld[1] = LarNew[1];
for (k=0; k<2; k++) { /* LOOP : gamma2 for 1st to 2nd subframes */
/* ---------------------------------------------------------- */
/* First criterion based on the first two Lars */
/* smooth == 1 ==> gamma2 can vary from 0.4 to 0.7 */
/* smooth == 0 ==> gamma2 is set to 0.6 */
/* */
/* Double threshold + hysteresis : */
/* if smooth = 1 */
/* if (CritLar0 < THRESH_L1) and (CritLar1 > THRESH_H1) */
/* smooth = 0 */
/* if smooth = 0 */
/* if (CritLar0 > THRESH_L2) or (CritLar1 < THRESH_H2) */
/* smooth = 1 */
/* ---------------------------------------------------------- */
CritLar0 = Lar[2*k];
CritLar1 = Lar[2*k+1];
if (smooth != 0) {
if ((sub(CritLar0,THRESH_L1)<0)&&( sub(CritLar1,THRESH_H1)>0)) {
smooth = 0;
}
}
else {
if ( (sub(CritLar0 ,THRESH_L2)>0) || (sub(CritLar1,THRESH_H2) <0) ) {
smooth = 1;
}
}
if (smooth == 0) {
/* ------------------------------------------------------ */
/* Second criterion based on the minimum distance between */
/* two successives LSPs */
/* */
/* gamma2[k] = -6.0 * pi * d_min + 1.0 */
/* */
/* with Lsfs normalized range 0.0 <= val <= 1.0 */
/* ------------------------------------------------------ */
gamma1[k] = GAMMA1_0;
if (k == 0) {
Lsf = LsfInt;
}
else {
Lsf = LsfNew;
}
d_min = sub(Lsf[1], Lsf[0]);
for (i=1; i<M-1; i++) {
temp = sub(Lsf[i+1],Lsf[i]);
if (sub(temp,d_min)<0) {
d_min = temp;
}
}
temp = mult(ALPHA, d_min);
temp = sub(BETA, temp);
temp = shl(temp, 5);
gamma2[k] = temp;
if (sub(gamma2[k] , GAMMA2_0_H)>0) {
gamma2[k] = GAMMA2_0_H;
}
if (sub(gamma2[k] ,GAMMA2_0_L)<0) {
gamma2[k] = GAMMA2_0_L;
}
}
else {
gamma1[k] = GAMMA1_1;
gamma2[k] = GAMMA2_1;
}
}
return;
}
/*-----------------------------------------------------------------*
* Functions vad *
* ~~~ *
* Input: *
* rc : reflection coefficient *
* lsf[] : unquantized lsf vector *
* r_h[] : upper 16-bits of the autocorrelation vector *
* r_l[] : lower 16-bits of the autocorrelation vector *
* exp_R0 : exponent of the autocorrelation vector *
* sigpp[] : preprocessed input signal *
* frm_count : frame counter *
* prev_marker : VAD decision of the last frame *
* pprev_marker : VAD decision of the frame before last frame *
* *
* Output: *
* *
* marker : VAD decision of the current frame *
* *
*-----------------------------------------------------------------*/
void vad(
Word16 rc,
Word16 *lsf,
Word16 *r_h,
Word16 *r_l,
Word16 exp_R0,
Word16 *sigpp,
Word16 frm_count,
Word16 prev_marker,
Word16 pprev_marker,
Word16 *marker)
{
/* scalar */
Word32 acc0;
Word16 i, j, exp, frac;
Word16 ENERGY, ENERGY_low, SD, ZC, dSE, dSLE, dSZC;
Word16 COEF, C_COEF, COEFZC, C_COEFZC, COEFSD, C_COEFSD;
/* compute the frame energy */
acc0 = L_Comp(r_h[0], r_l[0]);
Log2(acc0, &exp, &frac);
acc0 = Mpy_32_16(exp, frac, 9864);
i = sub(exp_R0, 1);
i = sub(i, 1);
acc0 = L_mac(acc0, 9864, i);
acc0 = L_shl(acc0, 11);
ENERGY = extract_h(acc0);
ENERGY = sub(ENERGY, 4875);
/* compute the low band energy */
acc0 = 0;
for (i=1; i<=NP; i++)
acc0 = L_mac(acc0, r_h[i], lbf_corr[i]);
acc0 = L_shl(acc0, 1);
acc0 = L_mac(acc0, r_h[0], lbf_corr[0]);
Log2(acc0, &exp, &frac);
acc0 = Mpy_32_16(exp, frac, 9864);
i = sub(exp_R0, 1);
i = sub(i, 1);
acc0 = L_mac(acc0, 9864, i);
acc0 = L_shl(acc0, 11);
ENERGY_low = extract_h(acc0);
ENERGY_low = sub(ENERGY_low, 4875);
/* compute SD */
acc0 = 0;
for (i=0; i<M; i++){
j = sub(lsf[i], MeanLSF[i]);
acc0 = L_mac(acc0, j, j);
}
SD = extract_h(acc0); /* Q15 */
/* compute # zero crossing */
ZC = 0;
for (i=ZC_START+1; i<=ZC_END; i++)
if (mult(sigpp[i-1], sigpp[i]) < 0)
ZC = add(ZC, 410); /* Q15 */
/* Initialize and update Mins */
if(sub(frm_count, 129) < 0){
if (sub(ENERGY, Min) < 0){
Min = ENERGY;
Prev_Min = ENERGY;
}
if((frm_count & 0x0007) == 0){
i = sub(shr(frm_count,3),1);
Min_buffer[i] = Min;
Min = MAX_16;
}
}
if((frm_count & 0x0007) == 0){
Prev_Min = Min_buffer[0];
for (i=1; i<16; i++){
if (sub(Min_buffer[i], Prev_Min) < 0)
Prev_Min = Min_buffer[i];
}
}
if(sub(frm_count, 129) >= 0){
if(((frm_count & 0x0007) ^ (0x0001)) == 0){
Min = Prev_Min;
Next_Min = MAX_16;
}
if (sub(ENERGY, Min) < 0)
Min = ENERGY;
if (sub(ENERGY, Next_Min) < 0)
Next_Min = ENERGY;
if((frm_count & 0x0007) == 0){
for (i=0; i<15; i++)
Min_buffer[i] = Min_buffer[i+1];
Min_buffer[15] = Next_Min;
Prev_Min = Min_buffer[0];
for (i=1; i<16; i++)
if (sub(Min_buffer[i], Prev_Min) < 0)
Prev_Min = Min_buffer[i];
}
}
if (sub(frm_count, INIT_FRAME) <= 0)
if(sub(ENERGY, 3072) < 0){
*marker = NOISE;
less_count++;
}
else{
*marker = VOICE;
acc0 = L_deposit_h(MeanE);
acc0 = L_mac(acc0, ENERGY, 1024);
MeanE = extract_h(acc0);
acc0 = L_deposit_h(MeanSZC);
acc0 = L_mac(acc0, ZC, 1024);
MeanSZC = extract_h(acc0);
for (i=0; i<M; i++){
acc0 = L_deposit_h(MeanLSF[i]);
acc0 = L_mac(acc0, lsf[i], 1024);
MeanLSF[i] = extract_h(acc0);
}
}
if (sub(frm_count, INIT_FRAME) >= 0){
if (sub(frm_count, INIT_FRAME) == 0){
acc0 = L_mult(MeanE, factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanE = extract_h(acc0);
acc0 = L_mult(MeanSZC, factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanSZC = extract_h(acc0);
for (i=0; i<M; i++){
acc0 = L_mult(MeanLSF[i], factor_fx[less_count]);
acc0 = L_shl(acc0, shift_fx[less_count]);
MeanLSF[i] = extract_h(acc0);
}
MeanSE = sub(MeanE, 2048); /* Q11 */
MeanSLE = sub(MeanE, 2458); /* Q11 */
}
dSE = sub(MeanSE, ENERGY);
dSLE = sub(MeanSLE, ENERGY_low);
dSZC = sub(MeanSZC, ZC);
if(sub(ENERGY, 3072) < 0)
*marker = NOISE;
else
*marker = MakeDec(dSLE, dSE, SD, dSZC);
v_flag = 0;
if((prev_marker==VOICE) && (*marker==NOISE) && (add(dSE,410) < 0)
&& (sub(ENERGY, 3072)>0)){
*marker = VOICE;
v_flag = 1;
}
if(flag == 1){
if((pprev_marker == VOICE) &&
(prev_marker == VOICE) &&
(*marker == NOISE) &&
(sub(abs_s(sub(prev_energy,ENERGY)), 614) <= 0)){
count_ext++;
*marker = VOICE;
v_flag = 1;
if(sub(count_ext, 4) <= 0)
flag=1;
else{
count_ext=0;
flag=0;
}
}
}
else
flag=1;
if(*marker == NOISE)
count_sil++;
if((*marker == VOICE) && (sub(count_sil, 10) > 0) &&
(sub(sub(ENERGY,prev_energy), 614) <= 0)){
*marker = NOISE;
count_sil=0;
}
if(*marker == VOICE)
count_sil=0;
if ((sub(sub(ENERGY, 614), MeanSE) < 0) && (sub(frm_count, 128) > 0)
&& (!v_flag) && (sub(rc, 19661) < 0))
*marker = NOISE;
if ((sub(sub(ENERGY,614),MeanSE) < 0) && (sub(rc, 24576) < 0)
&& (sub(SD, 83) < 0)){
count_update++;
if (sub(count_update, INIT_COUNT) < 0){
COEF = 24576;
C_COEF = 8192;
COEFZC = 26214;
C_COEFZC = 6554;
COEFSD = 19661;
C_COEFSD = 13017;
}
else
if (sub(count_update, INIT_COUNT+10) < 0){
COEF = 31130;
C_COEF = 1638;
COEFZC = 30147;
C_COEFZC = 2621;
COEFSD = 21299;
C_COEFSD = 11469;
}
else
if (sub(count_update, INIT_COUNT+20) < 0){
COEF = 31785;
C_COEF = 983;
COEFZC = 30802;
C_COEFZC = 1966;
COEFSD = 22938;
C_COEFSD = 9830;
}
else
if (sub(count_update, INIT_COUNT+30) < 0){
COEF = 32440;
C_COEF = 328;
COEFZC = 31457;
C_COEFZC = 1311;
COEFSD = 24576;
C_COEFSD = 8192;
}
else
if (sub(count_update, INIT_COUNT+40) < 0){
COEF = 32604;
C_COEF = 164;
COEFZC = 32440;
C_COEFZC = 328;
COEFSD = 24576;
C_COEFSD = 8192;
}
else{
COEF = 32604;
C_COEF = 164;
COEFZC = 32702;
C_COEFZC = 66;
COEFSD = 24576;
C_COEFSD = 8192;
}
/* compute MeanSE */
acc0 = L_mult(COEF, MeanSE);
acc0 = L_mac(acc0, C_COEF, ENERGY);
MeanSE = extract_h(acc0);
/* compute MeanSLE */
acc0 = L_mult(COEF, MeanSLE);
acc0 = L_mac(acc0, C_COEF, ENERGY_low);
MeanSLE = extract_h(acc0);
/* compute MeanSZC */
acc0 = L_mult(COEFZC, MeanSZC);
acc0 = L_mac(acc0, C_COEFZC, ZC);
MeanSZC = extract_h(acc0);
/* compute MeanLSF */
for (i=0; i<M; i++){
acc0 = L_mult(COEFSD, MeanLSF[i]);
acc0 = L_mac(acc0, C_COEFSD, lsf[i]);
MeanLSF[i] = extract_h(acc0);
}
}
if((sub(frm_count, 128) > 0) && (((sub(MeanSE,Min) < 0) &&
(sub(SD, 83) < 0)) || (sub(MeanSE,Min) > 2048))){
MeanSE = Min;
count_update = 0;
}
}
prev_energy = ENERGY;
}
inline ivect mult(ivect v, imat M) {
return matr2vec(mult(vec2matr(v), M), 0);
}
static int three_register_execute(UM_machine machine,
Um_instruction instruction)
{
unsigned op = Bitpack_getu(instruction, BITS_PER_OP_CODE, 28);
unsigned A = Bitpack_getu(instruction, BITS_PER_REG_NUM, 6);
unsigned B = Bitpack_getu(instruction, BITS_PER_REG_NUM, 3);
unsigned C = Bitpack_getu(instruction, BITS_PER_REG_NUM, 0);
switch(op){
case 0: //
movc(machine, A, B, C);
return 1;
case 1:
segl(machine, A, B, C);
return 1;
case 2:
segs(machine, A, B, C);
return 1;
case 3:
add(machine, A, B, C);
return 1;
case 4:
mult(machine, A, B, C);
return 1;
case 5:
divi(machine, A, B, C);
return 1;
case 6:
nand(machine, A, B, C);
return 1;
case 7:
return 0;
case 8:
map(machine, B, C);
return 1;
case 9:
unmap(machine, C);
return 1;
case 10:
out(machine, C);
return 1;
case 11:
in(machine, C);
return 1;
case 12:
loadp(machine, B, C);
return 1;
default:
return 0;
}
}
std::uint16_t VirtualCPU::executeInstructionAtAddress(std::uint16_t address) {
debugger_.pc_ = address; //HACK FOR NOW
switch (memoryController_.readAtAddress(address)) {
case 0:
return address + halt();
case 1:
return address + set(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2));
case 2:
return address + push(memoryController_.readAtAddress(address + 1));
case 3:
return address + pop(memoryController_.readAtAddress(address + 1));
case 4:
return address +
eq(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
memoryController_.readAtAddress(address + 3));
case 5:
return address + gt(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address +2),
memoryController_.readAtAddress(address + 3));
case 6:
return jump(memoryController_.readAtAddress(address + 1), address);
case 7:
return jt(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
address);
case 8:
return jf(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
address);
case 9:
return address + add(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
memoryController_.readAtAddress(address + 3));
case 10:
return address + mult(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
memoryController_.readAtAddress(address + 3));
case 11:
return address + mod(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
memoryController_.readAtAddress(address + 3));
case 12:
return address + and_i(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
memoryController_.readAtAddress(address + 3));
case 13:
return address + or_i(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2),
memoryController_.readAtAddress(address + 3));
case 14:
return address + not_i(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2));
case 15:
return address + rmem(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2));
case 16:
return address + wmem(memoryController_.readAtAddress(address + 1),
memoryController_.readAtAddress(address + 2));
case 17:
return call(memoryController_.readAtAddress(address + 1), address);
case 18:
return ret();
case 19:
return address + out(memoryController_.readAtAddress(address + 1));
case 21:
return address + noop();
case 20:
return address + in(memoryController_.readAtAddress(address + 1));
default:
std::cout << "CPU ERROR illegal op code: " << memoryController_.readAtAddress(address) << std::endl;
return address + 1;
}
}
void Dec_lag3(
Word16 index, /* input : received pitch index */
Word16 pit_min, /* input : minimum pitch lag */
Word16 pit_max, /* input : maximum pitch lag */
Word16 i_subfr, /* input : subframe flag */
Word16 *T0, /* output: integer part of pitch lag */
Word16 *T0_frac /* output: fractional part of pitch lag */
)
{
Word16 i;
Word16 T0_min, T0_max;
if (i_subfr == 0) /* if 1st subframe */
{
if (sub(index, 197) < 0)
{
/* *T0 = (index+2)/3 + 19 */
*T0 = add(mult(add(index, 2), 10923), 19);
/* *T0_frac = index - *T0*3 + 58 */
i = add(add(*T0, *T0), *T0);
*T0_frac = add(sub(index, i), 58);
}
else
{
*T0 = sub(index, 112);
*T0_frac = 0;
}
}
else /* second subframe */
{
/* find T0_min and T0_max for 2nd subframe */
T0_min = sub(*T0, 5);
if (sub(T0_min, pit_min) < 0)
{
T0_min = pit_min;
}
T0_max = add(T0_min, 9);
if (sub(T0_max, pit_max) > 0)
{
T0_max = pit_max;
T0_min = sub(T0_max, 9);
}
/* i = (index+2)/3 - 1 */
/* *T0 = i + t0_min; */
i = sub(mult(add(index, 2), 10923), 1);
*T0 = add(i, T0_min);
/* t0_frac = index - 2 - i*3; */
i = add(add(i, i), i);
*T0_frac = sub(sub(index, 2), i);
}
return;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
inline void mult_left(const quaternion& left) {
mult(left, *this);
}
DiagonalMatrix &DM::multT(const DiagonalMatrix &S, DiagonalMatrix &ans,
double scal) const {
return mult(S, ans, scal);
}
void Coder_ld8a(
g729a_encoder_state *state,
Word16 ana[] /* output : Analysis parameters */
)
{
/* 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[MP1], r_h[MP1]; /* Autocorrelations low and hi */
Word16 rc[M]; /* Reflection coefficients. */
Word16 lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe */
/* LP analysis */
Autocorr(state->p_window, M, r_h, r_l); /* Autocorrelations */
Lag_window(M, r_h, r_l); /* Lag windowing */
Levinson(r_h, r_l, Ap_t, rc); /* Levinson Durbin */
Az_lsp(Ap_t, lsp_new, state->lsp_old); /* From A(z) to lsp */
/* LSP quantization */
Qua_lsp(state, 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(state->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, state->lsp_old, M);
Copy(lsp_new_q, state->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], &(state->speech[0]), &(state->exc[0]), L_SUBFR);
Residu(&Aq_t[MP1], &(state->speech[L_SUBFR]), &(state->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, &(state->exc[0]), &(state->wsp[0]), L_SUBFR, state->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, &(state->exc[L_SUBFR]), &(state->wsp[L_SUBFR]), L_SUBFR, state->mem_w, 1);
}
/* Find open loop pitch lag */
T_op = Pitch_ol_fast(state->wsp, PIT_MAX, L_FRAME);
/* Range for closed loop pitch search in 1st subframe */
T0_min = T_op - 3;
T0_max = T0_min + 6;
if (T0_min < PIT_MIN)
{
T0_min = PIT_MIN;
T0_max = PIT_MIN + 6;
}
else if (T0_max > PIT_MAX)
{
T0_max = PIT_MAX;
T0_min = PIT_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, &(state->exc[i_subfr]), xn, L_SUBFR, state->mem_w0, 0);
/*---------------------------------------------------------------------*
* Closed-loop fractional pitch search *
*---------------------------------------------------------------------*/
T0 = Pitch_fr3_fast(&(state->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, &(state->exc[i_subfr]), y1, L_SUBFR, state->mem_zero, 0);
gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR);
/* clip pitch gain if taming is necessary */
taming = test_err(state, T0, T0_frac);
if( taming == 1){
if (gain_pit > GPCLIP) {
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 */
L_temp = ((Word32)y1[i] * gain_pit) << 2;
xn2[i] = sub(xn[i], extract_h(L_temp));
}
/*-----------------------------------------------------*
* - Innovative codebook search. *
*-----------------------------------------------------*/
index = ACELP_Code_A(xn2, h1, T0, state->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 *
*------------------------------------------------------------*/
state->sharp = gain_pit;
if (state->sharp > SHARPMAX) { state->sharp = SHARPMAX; }
else if (state->sharp < SHARPMIN) { state->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);
L_temp = (Word32)(state->exc[i+i_subfr]) * (Word32)gain_pit +
(Word32)code[i] * (Word32)gain_code;
L_temp <<= 2;
state->exc[i+i_subfr] = g_round(L_temp);
}
update_exc_err(state, gain_pit, T0);
for (i = L_SUBFR-M, j = 0; i < L_SUBFR; i++, j++)
{
temp = ((Word32)y1[i] * (Word32)gain_pit) >> 14;
k = ((Word32)y2[i] * (Word32)gain_code) >> 13;
state->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(&(state->old_speech[L_FRAME]), &(state->old_speech[0]), L_TOTAL-L_FRAME);
Copy(&(state->old_wsp[L_FRAME]), &(state->old_wsp[0]), PIT_MAX);
Copy(&(state->old_exc[L_FRAME]), &(state->old_exc[0]), PIT_MAX+L_INTERPOL);
}
void loop(void)
{
const float spd1=30.0f*dt();
const float spd2=15.0f*dt();
float r=0.0f;
float dc=0.0f, ds=0.0f;
float tc=0.0f, ts=0.0f;
/*viewport(0,0,nsw,nsh);*/
use_basic();
send_lpview(pview);
if(mm() || mw()!=0.0f)
{
if(mm())
{
dir+=mx()*spd1;
if(dir<-0.0f)
{
dir+=360.0f;
ndir+=360.0f;
}
else if(dir>360.0f)
{
dir-=360.0f;
ndir-=360.0f;
}
tilt-=my()*spd1;
if(tilt<0.001f) tilt=0.001f;
else if(tilt>100.0f) tilt=100.0f;
}
if(mw()!=0.0f)
{
zoom-=mw();
if(zoom<7.0f) zoom=7.0f;
else if(zoom>12.0f) zoom=12.0f;
}
}
if(mp(3)) zoom=10.0f;
ndir-=(ndir-dir)*spd2;
r=d2r(ndir);
dc=cosf(r);
ds=sinf(r);
ntilt-=(ntilt-tilt)*spd2;
r=d2r(ntilt);
tc=cosf(r);
ts=sinf(r);
nzoom-=(nzoom-zoom)*spd2;
look(view,v3(ds*ts*nzoom,dc*ts*nzoom,3.0f+(tc*nzoom)),v3(0.0f,0.0f,3.0f));
mult(pview,proj,view);
send_pview(pview);
/* ----- */
use_fb(fbos[0]);
clear();
/* ----- */
use_tex(area_tex);
/* ----- */
draw_vbo(&area_mod);
use_fb(0);
blit_fb(fbos[0],fbos[1],nsw,nsh);
/* ----- */
use_fb(fbos[2]);
set_drawbufs(2);
clear();
draw_vbo(&area_mod);
use_fb(0);
use_mblur();
use_tex(texs[0]); /* ----- */
active_tex(GL_TEXTURE1,texs[2]);
default_tex();
use_fb(fbos[3]);
set_drawbufs(1);
clear();
quad();
use_fb(0);
active_tex(GL_TEXTURE1,0);
default_tex();
/*viewport(0,0,sw,sh);*/
use_vig();
use_tex(texs[3]);
clear();
quad();
if(kp(SDL_SCANCODE_DELETE) || kp(SDL_SCANCODE_ESCAPE))
quit();
}
/*inline*/ vertex3f vertex3f::operator*(const float op) const {
return mult(op);
}
int main(int argc, char**argv)
{
int block_sz = atoi(argv[1]);
int blocks_nr = atoi(argv[2]);
int edge_sz = atoi(argv[3]);
std::vector<Block> bs(blocks_nr, Block(block_sz,edge_sz) );
Edge e(edge_sz,blocks_nr,block_sz);
for (int i=0; i < bs.size(); ++i)
bs[i].fill();
e.fill();
// for (int i=0; i < bs.size(); ++i)
// std::cout << bs[i] << '\n';
// std::cout << e << '\n';
std::vector<Block> bs0 = bs;
Edge e0 = e;
double total_time=0.0;
clock_t st, end;
st = clock();
for (int i=0; i < bs.size(); ++i)
bs[i].fwd();
end = clock();
total_time += double((end - st)) / CLOCKS_PER_SEC;
for (int b=0; b < bs.size(); ++b)
{
for (int br=0; br < bs[b].rows; ++br)
{
double& diag = bs[b](br,br);
RowNO r;
r.v = &diag;
r.sz = bs[b].cols-br;
r.row_no = bs[b].rows*b + br;
st = clock();
elim_col(e,r);
end = clock();
total_time += double((end - st)) / CLOCKS_PER_SEC;
}
}
st = clock();
e.fwd();
e.bwd();
for (int i=0; i < bs.size(); ++i){
bs[i].bwd(e.edge_soln);
}
end = clock();
total_time += double((end - st)) / CLOCKS_PER_SEC;
std::cout << "elapsed " << total_time << '\n';
std::vector<double> soln(bs[0].rows*bs.size() + edge_sz);
for (int i=0; i < bs.size(); ++i)
for (int k=0; k < bs[i].sol.size(); ++k)
soln[i*block_sz+k] = bs[i].sol[k];
for (int k=0; k < e.edge_soln.size(); ++k)
soln[blocks_nr*block_sz+k] = e.edge_soln[k];
// for (int k=0; k < soln.size(); ++k)
// std::cout << soln[k] << '\n';
std::vector<double> Rhs;
for(int i = 0; i < bs0.size(); ++i)
{
for(int r=0; r < bs0[i].rows; ++r)
Rhs.push_back(bs0[i](r,bs0[i].cols-1));
}
for(int i = 0; i < e0.rows; ++i)
{
Rhs.push_back(e0(i,e0.cols-1));
}
std::vector<double> rhs = mult(bs0,e0,soln);
double residue=0.0;
for (int i=0; i < rhs.size(); ++i)
residue += fabs(rhs[i] - Rhs[i]);
std::cout << "\n\nres = " << residue << '\n';
}
Node Node::operator*(const Node &node) const {
return mult(node);
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
inline void mult_right(const quaternion& right) {
mult(*this, right);
}
void D_plsf_3(
D_plsfState *st, /* i/o: State struct */
enum Mode mode, /* i : coder mode */
Word16 bfi, /* i : bad frame indicator (set to 1 if a */
/* bad frame is received) */
Word16 * indice, /* i : quantization indices of 3 submatrices, Q0 */
Word16 * lsp1_q /* o : quantized 1st LSP vector, Q15 */
)
{
Word16 i, index;
Word16 *p_cb1, *p_cb2, *p_cb3, *p_dico, temp;
Word16 lsf1_r[M];
Word16 lsf1_q[M];
if (bfi != 0) /* if bad frame */
{
/* use the past LSFs slightly shifted towards their mean */
for (i = 0; i < M; i++)
{
/* lsfi_q[i] = ALPHA*past_lsf_q[i] + ONE_ALPHA*mean_lsf[i]; */
lsf1_q[i] = add(mult(st->past_lsf_q[i], ALPHA),
mult(mean_lsf[i], ONE_ALPHA));
}
/* estimate past quantized residual to be used in next frame */
if (mode != MRDTX) {
for (i = 0; i < M; i++) {
/* temp = mean_lsf[i] + past_r2_q[i] * PRED_FAC; */
temp = add(mean_lsf[i], mult(st->past_r_q[i], pred_fac[i]));
st->past_r_q[i] = sub(lsf1_q[i], temp);
}
} else {
for (i = 0; i < M; i++) {
/* temp = mean_lsf[i] + past_r2_q[i]; */
temp = add(mean_lsf[i], st->past_r_q[i]);
st->past_r_q[i] = sub(lsf1_q[i], temp);
}
}
}
else /* if good LSFs received */
{
if (mode == MR475 || mode == MR515)
{ /* MR475, MR515 */
p_cb1 = dico1_lsf;
p_cb2 = dico2_lsf;
p_cb3 = mr515_3_lsf;
}
else if (mode == MR795)
{ /* MR795 */
p_cb1 = mr795_1_lsf;
p_cb2 = dico2_lsf;
p_cb3 = dico3_lsf;
}
else
{ /* MR59, MR67, MR74, MR102, MRDTX */
p_cb1 = dico1_lsf;
p_cb2 = dico2_lsf;
p_cb3 = dico3_lsf;
}
/* decode prediction residuals from 3 received indices */
index = *indice++;
p_dico = &p_cb1[/*add(index, add(index, index))*/ 3 * index];
lsf1_r[0] = *p_dico++;
lsf1_r[1] = *p_dico++;
lsf1_r[2] = *p_dico++;
index = *indice++;
if (mode == MR475 || mode == MR515)
{ /* MR475, MR515 only using every second entry */
/*index = shl(index,1);*/
index <<= 1;
}
p_dico = &p_cb2[add(index, add(index, index))];
lsf1_r[3] = *p_dico++;
lsf1_r[4] = *p_dico++;
lsf1_r[5] = *p_dico++;
index = *indice++;
p_dico = &p_cb3[shl(index, 2)];
lsf1_r[6] = *p_dico++;
lsf1_r[7] = *p_dico++;
lsf1_r[8] = *p_dico++;
lsf1_r[9] = *p_dico++;
/* Compute quantized LSFs and update the past quantized residual */
if (mode != MRDTX)
for (i = 0; i < M; i++) {
temp = add(mean_lsf[i], mult(st->past_r_q[i], pred_fac[i]));
lsf1_q[i] = add(lsf1_r[i], temp);
st->past_r_q[i] = lsf1_r[i];
}
else
for (i = 0; i < M; i++) {
temp = add(mean_lsf[i], st->past_r_q[i]);
lsf1_q[i] = add(lsf1_r[i], temp);
st->past_r_q[i] = lsf1_r[i];
}
}
/* verification that LSFs has minimum distance of LSF_GAP Hz */
Reorder_lsf(lsf1_q, LSF_GAP, M);
Copy (lsf1_q, st->past_lsf_q, M);
/* convert LSFs to the cosine domain */
Lsf_lsp(lsf1_q, lsp1_q, M);
}
void pulsesequence() {
/* Internal variable declarations *************************/
int shapelist90,shapelist180;
double seqtime,tau1,tau2,tau3,te1_delay,te2_delay,te3_delay,tr_delay;
double freq90[MAXNSLICE], freq180[MAXNSLICE];
/* Diffusion variables */
double te1, te1min, del1, del2, del3, del4;
double te_diff1, te_diff2, tmp1, tmp2;
double diffamp;
char diffpat[MAXSTR];
/* Navigator variables */
double etlnav;
/* Variable crushers */
double cscale;
double vcrush; // flag
/* Diffusion parameters */
#define MAXDIR 1024 /* Will anybody do more than 1024 directions or b-values? */
double roarr[MAXDIR], pearr[MAXDIR], slarr[MAXDIR];
int nbval, /* Total number of bvalues*directions */
nbro, nbpe, nbsl,
i;
double bro[MAXDIR], bpe[MAXDIR], bsl[MAXDIR], /* b-values along RO, PE, SL */
brs[MAXDIR], brp[MAXDIR], bsp[MAXDIR], /* and the cross-terms */
btrace[MAXDIR], /* and the trace */
max_bval=0,
dcrush, dgss2, /* "delta" for crusher and gss2 gradients */
Dro, Dcrush, Dgss2; /* "DELTA" for readout, crusher and gss2 gradients */
/* Real-time variables used in this sequence **************/
int vpe_ctr = v1; // PE loop counter
int vpe_mult = v2; // PE multiplier, ranges from -PE/2 to PE/2
int vpe2_ctr = v3; // PE loop counter
int vpe2_mult = v4; // PE multiplier, ranges from -PE/2 to PE/2
int vpe2_offset = v5;
int vpe2_steps = v6;
int vms_slices = v7; // Number of slices
int vms_ctr = v8; // Slice loop counter
int vseg = v9; // Number of ETL segments
int vseg_ctr = v10; // Segment counter
int vetl = v11; // Echo train length
int vetl_ctr = v12; // Echo train loop counter
int vssc = v13; // Compressed steady-states
int vtrimage = v14; // Counts down from nt, trimage delay when 0
int vacquire = v15; // Argument for setacqvar, to skip steady state acquires
int vphase180 = v16; // phase of 180 degree refocusing pulse
int vetl_loop = v17; // Echo train length MINUS ONE, used on etl loop
int vnav = v18; // Echo train length
int vcr_ctr = v19; // variable crusher, index into table
int vcr1 = v20; // multiplier along RO
int vcr2 = v21; // multiplier along PE
int vcr3 = v22; // multiplier along SL
int vetl1 = v23; // = etl-1, determine navigator echo location in echo loop
int vcr_reset = v24; // check for navigator echoes, reset crushers
/* Initialize paramaters **********************************/
get_parameters();
te1 = getval("te1"); /* te1 is the echo time for the first echo */
cscale = getval("cscale"); /* Scaling factor on first 180 crushers */
vcrush = getval("vcrush"); /* Variable crusher or set amplitude? */
getstr("diffpat",diffpat);
/* Load external PE table ********************************/
if (strcmp(petable,"n") && strcmp(petable,"N") && strcmp(petable,"")) {
loadtable(petable);
} else {
abort_message("petable undefined");
}
/* Hold variable crushers in tables 5, 6, 7 */
settable(t5,8,crro);
settable(t6,8,crpe);
settable(t7,8,crss);
seqtime = 0.0;
espmin = 0.0;
/* RF Power & Bandwidth Calculations **********************/
init_rf(&p1_rf,p1pat,p1,flip1,rof1,rof2);
init_rf(&p2_rf,p2pat,p2,flip2,rof1,rof2);
// shape_rf(&p1_rf,"p1",p1pat,p1,flip1,rof1,rof2);
// shape_rf(&p2_rf,"p2",p2pat,p2,flip2,rof1,rof2);
calc_rf(&p1_rf,"tpwr1","tpwr1f");
calc_rf(&p2_rf,"tpwr2","tpwr2f");
/* Initialize gradient structures *************************/
init_readout(&ro_grad,"ro",lro,np,sw);
init_readout_refocus(&ror_grad,"ror");
init_phase(&pe_grad,"pe",lpe,nv);
init_phase(&pe2_grad,"pe2",lpe2,nv2);
init_slice(&ss_grad,"ss",thk); /* NOTE assume same band widths for p1 and p2 */
init_slice(&ss2_grad,"ss2",thk); /* not butterfly, want to scale crushers w/ echo */
init_slice_refocus(&ssr_grad,"ssr");
/* Gradient calculations **********************************/
calc_readout(&ro_grad,WRITE,"gro","sw","at");
calc_readout_refocus(&ror_grad,&ro_grad,NOWRITE,"gror");
calc_phase(&pe_grad,NOWRITE,"gpe","tpe");
calc_phase(&pe2_grad,NOWRITE,"gpe2","tpe2");
calc_slice(&ss_grad,&p1_rf,WRITE,"gss");
calc_slice(&ss2_grad,&p1_rf,WRITE,"");
calc_slice_refocus(&ssr_grad,&ss_grad,WRITE,"gssr");
/* Equalize refocus and PE gradient durations *************/
calc_sim_gradient(&ror_grad,&pe_grad,&pe2_grad,0.0,WRITE);
/* Variable crusher */
init_generic(&crush_grad,"crush",gcrush,tcrush);
calc_generic(&crush_grad,WRITE,"","");
/* Create optional prepulse events ************************/
if (sat[0] == 'y') create_satbands();
if (fsat[0] == 'y') create_fatsat();
if (mt[0] == 'y') create_mtc();
/* Optional Diffusion gradient */
if (diff[0] == 'y') {
init_generic(&diff_grad,"diff",gdiff,tdelta);
if (!strcmp("sine",diffpat)) {
diff_grad.shape = SINE;
diffamp = gdiff*1;
}
/* adjust duration, so tdelta is from start ramp up to start ramp down */
if ((ix == 1) && (diff_grad.shape == TRAPEZOID)) {
calc_generic(&diff_grad,NOWRITE,"","");
diff_grad.duration += diff_grad.tramp;
}
calc_generic(&diff_grad,WRITE,"","");
}
/* Set up frequency offset pulse shape list ********/
offsetlist(pss,ss_grad.amp,0,freq90,ns,seqcon[1]);
offsetlist(pss,ss2_grad.ssamp,0,freq180,ns,seqcon[1]);
shapelist90 = shapelist(p1_rf.pulseName,ss_grad.rfDuration, freq90, ns,0,seqcon[1]);
shapelist180 = shapelist(p2_rf.pulseName,ss2_grad.rfDuration,freq180,ns,0,seqcon[1]);
/* same slice selection gradient and RF pattern used */
if (ss_grad.rfFraction != 0.5)
abort_message("RF pulse must be symmetric (RF fraction = %.2f)",ss_grad.rfFraction);
if (ro_grad.echoFraction != 1)
abort_message("Echo Fraction must be 1");
/*****************************************************/
/* TIMING FOR ECHOES *********************************/
/*****************************************************/
/* First echo time, without diffusion */
tau1 = ss_grad.rfCenterBack + ssr_grad.duration + crush_grad.duration + ss2_grad.rfCenterFront;
tau2 = ss2_grad.rfCenterBack + crush_grad.duration + pe_grad.duration + ro_grad.timeToEcho;
te1min = 2*MAX(tau1,tau2);
if (te1 < te1min + 2*4e-6) {
abort_message("First echo time too small, minimum is %.2fms\n",(te1min+2*4e-6)*1000);
}
/* Each half-echo period in the ETL loop ********/
tau3 = ro_grad.timeFromEcho + pe_grad.duration + crush_grad.duration + ss2_grad.rfCenterFront;
espmin = 2*MAX(tau2,tau3); // Minimum echo spacing
if (minesp[0] == 'y') {
esp = espmin + 2*4e-6;
putvalue("esp",esp);
}
if (esp - (espmin + 2*4e-6) < -12.5e-9) {
abort_message("Echo spacing too small, minimum is %.2fms\n",(espmin+2*4e-6)*1000);
}
te1_delay = te1/2.0 - tau1;
te2_delay = esp/2.0 - tau2;
te3_delay = esp/2.0 - tau3;
/*****************************************************/
/* TIMING FOR DIFFUSION ******************************/
/*****************************************************/
del1 = te1/2.0 - tau1;
del2 = 0;
del3 = te1/2.0 - tau2;
del4 = 0;
if (diff[0] == 'y') {
tau1 += diff_grad.duration;
tau2 += diff_grad.duration;
te1min = 2*MAX(tau1,tau2);
if (te1 < te1min + 4*4e-6) { /* te1 is split into 4 delays, each of which must be >= 4us */
abort_message("ERROR %s: First echo time too small, minimum is %.2fms\n",seqfil,te1min*1000);
}
/* te1 is the echo time for the first echo */
te_diff1 = te1/2 - tau1; /* Available time in first half of first echo */
te_diff2 = te1/2 - tau2; /* Available time in second half of first echo */
tmp1 = ss2_grad.duration + 2*crush_grad.duration; /* duration of 180 block */
/* Is tDELTA long enough? */
if (tDELTA < diff_grad.duration + tmp1)
abort_message("DELTA too short, increase to %.2fms",
(diff_grad.duration + tmp1)*1000);
/* Is tDELTA too long? */
tmp2 = diff_grad.duration + te_diff1 + tmp1 + te_diff2;
if (tDELTA > tmp2) {
abort_message("DELTA too long, increase te1 to %.2fms",
(te1 + (tDELTA-tmp2))*1000);
}
/* First attempt to put lobes right after slice select, ie del1 = 0 */
del1 = 4e-6; /* At least 4us after setting power for 180 */
del2 = te_diff1 - del1;
del3 = tDELTA - (diff_grad.duration + del2 + tmp1);
if (del3 < 4e-6) { /* shift diffusion block towards acquisition */
del3 = 4e-6;
del2 = tDELTA - (diff_grad.duration + tmp1 + del3);
}
del1 = te_diff1 - del2;
del4 = te_diff2 - del3;
if (fabs(del4) < 12.5e-9) del4 = 0;
}
te = te1 + (kzero-1)*esp; // Return effective TE
putvalue("te",te);
/* How many echoes in the echo loop, including navigators? */
etlnav = (etl-1)+(navigator[0]=='y')*2.0;
/* Minimum TR **************************************/
seqtime = 4e-6 + 2*nseg*ns*4e-6; /* count all the 4us delays */
seqtime += ns*(ss_grad.duration/2 + te1 + (etlnav)*esp + ro_grad.timeFromEcho + pe_grad.duration + te3_delay);
/* Increase TR if any options are selected****************/
if (sat[0] == 'y') seqtime += ns*satTime;
if (fsat[0] == 'y') seqtime += ns*fsatTime;
if (mt[0] == 'y') seqtime += ns*mtTime;
trmin = seqtime + ns*4e-6; /* Add 4us to ensure that tr_delay is always >= 4us */
if (mintr[0] == 'y'){
tr = trmin;
putvalue("tr",tr+1e-6);
}
if (tr < trmin) {
abort_message("TR too short. Minimum TR = %.2fms\n",trmin*1000);
}
tr_delay = (tr - seqtime)/ns;
/* Set number of segments for profile or full image **********/
nseg = prep_profile(profile[0],nv/etl,&pe_grad,&per_grad);
pe2_steps = prep_profile(profile[1],nv2,&pe2_grad,&pe2r_grad);
/* Calculate total scan time */
g_setExpTime(tr*(nt*nseg*pe2_steps*arraydim + ssc));
/***************************************************/
/* CALCULATE B VALUES ******************************/
if (diff[0] == 'y') {
/* Get multiplication factors and make sure they have same # elements */
/* All this is only necessary because putCmd only work for ix==1 */
nbro = (int) getarray("dro",roarr); nbval = nbro;
nbpe = (int) getarray("dpe",pearr); if (nbpe > nbval) nbval = nbpe;
nbsl = (int) getarray("dsl",slarr); if (nbsl > nbval) nbval = nbsl;
if ((nbro != nbval) && (nbro != 1))
abort_message("%s: Number of directions/b-values must be the same for all axes (readout)",seqfil);
if ((nbpe != nbval) && (nbpe != 1))
abort_message("%s: Number of directions/b-values must be the same for all axes (phase)",seqfil);
if ((nbsl != nbval) && (nbsl != 1))
abort_message("%s: Number of directions/b-values must be the same for all axes (slice)",seqfil);
if (nbro == 1) for (i = 1; i < nbval; i++) roarr[i] = roarr[0];
if (nbpe == 1) for (i = 1; i < nbval; i++) pearr[i] = pearr[0];
if (nbsl == 1) for (i = 1; i < nbval; i++) slarr[i] = slarr[0];
}
else {
nbval = 1;
roarr[0] = 0;
pearr[0] = 0;
slarr[0] = 0;
}
for (i = 0; i < nbval; i++) {
dcrush = crush_grad.duration; //"delta" for crusher
Dcrush = dcrush + ss_grad.duration; //"DELTA" for crusher
/* Readout */
Dro = ror_grad.duration;
bro[i] = bval(gdiff*roarr[i],tdelta,tDELTA);
bro[i] += bval(ro_grad.amp,ro_grad.timeToEcho,Dro);
bro[i] += bval(crush_grad.amp,dcrush,Dcrush);
bro[i] += bval_nested(gdiff*roarr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
/* Slice */
dgss2 = Dgss2 = ss_grad.rfCenterFront;
bsl[i] = bval(gdiff*slarr[i],tdelta,tDELTA);
bsl[i] += bval(crush_grad.amp,dcrush,Dcrush);
bsl[i] += bval(ss2_grad.ssamp,dgss2,Dgss2);
bsl[i] += bval_nested(gdiff*slarr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
bsl[i] += bval_nested(gdiff*slarr[i],tdelta,tDELTA,ss2_grad.ssamp,dgss2,Dgss2);
bsl[i] += bval_nested(ss2_grad.ssamp,dgss2,Dgss2,
crush_grad.amp,dcrush,Dcrush);
/* Phase */
bpe[i] = bval(gdiff*pearr[i],tdelta,tDELTA);
bpe[i] += bval(crush_grad.amp,dcrush,Dcrush);
bpe[i] += bval_nested(gdiff*pearr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
/* Readout/Slice Cross-terms */
brs[i] = bval2(gdiff*roarr[i],gdiff*slarr[i],tdelta,tDELTA);
brs[i] += bval2(crush_grad.amp,
crush_grad.amp,dcrush,Dcrush);
brs[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
brs[i] += bval_cross(gdiff*slarr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
brs[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA,
ss2_grad.ssamp,dgss2,Dgss2);
brs[i] += bval_cross(crush_grad.amp,dcrush,Dcrush,
ss2_grad.ssamp,dgss2,Dgss2);
/* Readout/Phase Cross-terms */
brp[i] = bval2(gdiff*roarr[i],gdiff*pearr[i],tdelta,tDELTA);
brp[i] += bval2(crush_grad.amp,
crush_grad.amp,dcrush,Dcrush);
brp[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
brp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
/* Slice/Phase Cross-terms */
bsp[i] = bval2(gdiff*pearr[i],gdiff*slarr[i],tdelta,tDELTA);
bsp[i] += bval2(crush_grad.amp,
crush_grad.amp,dcrush,Dcrush);
bsp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
bsp[i] += bval_cross(gdiff*slarr[i],tdelta,tDELTA,
crush_grad.amp,dcrush,Dcrush);
bsp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA,
ss2_grad.ssamp,dgss2,Dgss2);
bsp[i] += bval_cross(crush_grad.amp,dcrush,Dcrush,
ss2_grad.ssamp,dgss2,Dgss2);
btrace[i] = (bro[i]+bsl[i]+bpe[i]);
if (max_bval < btrace[i]) {
max_bval = (bro[i]+bsl[i]+bpe[i]);
}
} /* End for-all-directions */
putarray("bvalrr",bro,nbval);
putarray("bvalpp",bpe,nbval);
putarray("bvalss",bsl,nbval);
putarray("bvalrp",brp,nbval);
putarray("bvalrs",brs,nbval);
putarray("bvalsp",bsp,nbval);
putarray("bvalue",btrace,nbval);
putvalue("max_bval",max_bval);
/* Shift DDR for pro *******************************/
roff = -poffset(pro,ro_grad.roamp);
/* PULSE SEQUENCE *************************************/
if (ix == 1) grad_advance(tep);
initval(fabs(ssc),vssc); // Compressed steady-state counter
setacqvar(vacquire); // Control acquisition through vacquire
assign(one,vacquire); // Turn on acquire when vacquire is zero
/* Phase cycle: Alternate 180 phase to cancel residual FID */
mod2(ct,vphase180); // 0101
dbl(vphase180,vphase180); // 0202
add(vphase180,one,vphase180); // 1313 Phase difference from 90
add(vphase180,oph,vphase180);
obsoffset(resto);
delay(4e-6);
initval(nseg,vseg);
initval(pe2_steps/2.0,vpe2_offset);
initval(etl,vetl);
initval(etl-1,vetl1);
peloop2(seqcon[3],pe2_steps,vpe2_steps,vpe2_ctr);
/* Use standard encoding order for 2nd PE dimension */
sub(vpe2_ctr,vpe2_offset,vpe2_mult);
loop(vseg,vseg_ctr);
/* Compressed steady-states: 1st array & transient, all arrays if ssc is negative */
if ((ix > 1) && (ssc > 0))
assign(zero,vssc);
sub(vseg_ctr,vssc,vseg_ctr); // vpe_ctr counts up from -ssc
assign(zero,vssc);
ifzero(vseg_ctr);
assign(zero,vacquire); // Start acquiring when vseg_ctr reaches zero
endif(vseg_ctr);
msloop(seqcon[1],ns,vms_slices,vms_ctr);
if (ticks) {
xgate(ticks);
grad_advance(tep);
}
sp1on(); delay(4e-6); sp1off(); // Scope trigger
/* Prepulse options ***********************************/
if (sat[0] == 'y') satbands();
if (fsat[0] == 'y') fatsat();
if (mt[0] == 'y') mtc();
/* 90 degree pulse ************************************/
rotate();
obspower(p1_rf.powerCoarse);
obspwrf(p1_rf.powerFine);
delay(4e-6);
obl_shapedgradient(ss_grad.name,ss_grad.duration,0,0,ss_grad.amp,NOWAIT);
delay(ss_grad.rfDelayFront);
shapedpulselist(shapelist90,ss_grad.rfDuration,oph,rof1,rof2,seqcon[1],vms_ctr);
delay(ss_grad.rfDelayBack);
/* Read dephase and Slice refocus *********************/
obl_shapedgradient(ssr_grad.name,ssr_grad.duration,0.0,0.0,-ssr_grad.amp,WAIT);
/* First half-TE delay ********************************/
obspower(p2_rf.powerCoarse);
obspwrf(p2_rf.powerFine);
delay(del1);
/* DIFFUSION GRADIENT */
if (diff[0] == 'y')
obl_shapedgradient(diff_grad.name,diff_grad.duration,diff_grad.amp*dro,diff_grad.amp*dpe,diff_grad.amp*dsl,WAIT);
delay(del2);
/*****************************************************/
/* FIRST ECHO OUTSIDE LOOP ***************************/
/*****************************************************/
ifzero(vacquire); // real acquisition, get PE multiplier from table
mult(vseg_ctr,vetl,vpe_ctr);
getelem(t1,vpe_ctr,vpe_mult);
elsenz(vacquire); // steady state scan
assign(zero,vpe_mult);
endif(vacquire);
/* Variable crusher */
assign(zero,vcr_ctr);
getelem(t5,vcr_ctr,vcr1);
getelem(t6,vcr_ctr,vcr2);
getelem(t7,vcr_ctr,vcr3);
if(vcrush)
phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
crush_grad.duration,
(double)0,(double)0,(double)0, // base levels
crush_grad.amp*cscale,crush_grad.amp*cscale,crush_grad.amp*cscale, // step size
vcr1,vcr2,vcr3, // multipliers
(double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers
1,WAIT,0);
else
obl_shapedgradient(crush_grad.name,crush_grad.duration,
crush_grad.amp,crush_grad.amp,crush_grad.amp,WAIT);
/* 180 degree pulse *******************************/
obl_shapedgradient(ss2_grad.name,ss2_grad.duration,0,0,ss2_grad.amp,NOWAIT);
delay(ss2_grad.rfDelayFront);
shapedpulselist(shapelist180,ss2_grad.rfDuration,vphase180,rof1,rof2,seqcon[1],vms_ctr);
delay(ss2_grad.rfDelayBack);
if (vcrush)
phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
crush_grad.duration,
(double)0,(double)0,(double)0, // base levels
crush_grad.amp*cscale,crush_grad.amp*cscale,crush_grad.amp*cscale, // step size
vcr1,vcr2,vcr3, // multipliers
(double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers
1,WAIT,0);
else
obl_shapedgradient(crush_grad.name,crush_grad.duration,
crush_grad.amp,crush_grad.amp,crush_grad.amp,WAIT);
delay(del3);
/* DIFFUSION GRADIENT */
if (diff[0] == 'y')
obl_shapedgradient(diff_grad.name,diff_grad.duration,diff_grad.amp*dro,diff_grad.amp*dpe,diff_grad.amp*dsl,WAIT);
delay(del4);
/* Phase-encode gradient ******************************/
pe2_shapedgradient(pe_grad.name,pe_grad.duration,-ror_grad.amp,0,0,
-pe_grad.increment,-pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);
/* Readout gradient ************************************/
obl_shapedgradient(ro_grad.name,ro_grad.duration,ro_grad.roamp,0,0,NOWAIT);
delay(ro_grad.atDelayFront);
/* Acquire data ****************************************/
startacq(10e-6);
acquire(np,1.0/sw);
endacq();
delay(ro_grad.atDelayBack);
/* Rewinding phase-encode gradient ********************/
pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0,
pe_grad.increment,pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);
/* Second half-TE delay *******************************/
delay(te3_delay);
/*****************************************************/
/* LOOP THROUGH THE REST OF ETL **********************/
/*****************************************************/
peloop(seqcon[2],etlnav,vetl_loop,vetl_ctr);
ifzero(vacquire); // real acquisition, get PE multiplier from table
mult(vseg_ctr,vetl,vpe_ctr);
add(vpe_ctr,vetl_ctr,vpe_ctr);
add(vpe_ctr,one,vpe_ctr);
getelem(t1,vpe_ctr,vpe_mult);
elsenz(vacquire); // steady state scan
assign(zero,vpe_mult);
endif(vacquire);
/* But don't phase encode navigator echoes */
ifrtGE(vetl_ctr,vetl1,vnav);
assign(zero,vpe_mult);
endif(vnav);
/* Variable crusher */
incr(vcr_ctr); /* Get next crusher level */
/* Except if we're doing navigators, start over */
sub(vetl1,vetl_ctr,vcr_reset);
ifzero(vcr_reset);
assign(zero,vcr_ctr);
endif(vcr_reset);
getelem(t5,vcr_ctr,vcr1);
getelem(t6,vcr_ctr,vcr2);
getelem(t7,vcr_ctr,vcr3);
phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
crush_grad.duration,
(double)0,(double)0,(double)0, // base levels
crush_grad.amp,crush_grad.amp,crush_grad.amp, // step size
vcr1,vcr2,vcr3, // multipliers
(double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers
1,WAIT,0);
/* 180 degree pulse *******************************/
obl_shapedgradient(ss2_grad.name,ss2_grad.duration,0,0,ss2_grad.amp,NOWAIT);
delay(ss2_grad.rfDelayFront);
shapedpulselist(shapelist180,ss2_grad.rfDuration,vphase180,rof1,rof2,seqcon[1],vms_ctr);
delay(ss2_grad.rfDelayBack);
/* Variable crusher */
phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name,
crush_grad.duration,
(double)0,(double)0,(double)0, // base levels
crush_grad.amp,crush_grad.amp,crush_grad.amp, // step size
vcr1,vcr2,vcr3, // multipliers
(double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers
1,WAIT,0);
/* Phase-encode gradient ******************************/
pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0,
-pe_grad.increment,-pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);
/* Second half-TE period ******************************/
delay(te2_delay);
/* Readout gradient ************************************/
obl_shapedgradient(ro_grad.name,ro_grad.duration,ro_grad.roamp,0,0,NOWAIT);
delay(ro_grad.atDelayFront);
startacq(10e-6);
acquire(np,1.0/sw);
endacq();
delay(ro_grad.atDelayBack);
/* Rewinding phase-encode gradient ********************/
pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0,
pe_grad.increment,pe2_grad.increment,vpe_mult,vpe2_mult,WAIT);
/* Second half-TE delay *******************************/
delay(te3_delay);
endpeloop(seqcon[2],vetl_ctr);
/* Relaxation delay ***********************************/
if (!trtype)
delay(tr_delay);
endmsloop(seqcon[1],vms_ctr);
if (trtype)
delay(ns*tr_delay);
endloop(vseg_ctr);
endpeloop(seqcon[3],vpe2_ctr);
/* Inter-image delay **********************************/
sub(ntrt,ct,vtrimage);
decr(vtrimage);
ifzero(vtrimage);
delay(trimage);
endif(vtrimage);
}
Word16 code_2i40_9bits(
Word16 subNr, /* i : subframe number */
Word16 x[], /* i : target vector */
Word16 h[], /* i : impulse response of weighted synthesis */
/* filter h[-L_subfr..-1] must be set to 0. */
Word16 T0, /* i : Pitch lag */
Word16 pitch_sharp, /* i : Last quantized pitch gain */
Word16 code[], /* o : Innovative codebook */
Word16 y[], /* o : filtered fixed codebook excitation */
Word16 * sign, /* o : Signs of 2 pulses */
Flag * pOverflow /* o : Flag set when overflow occurs */
)
{
Word16 codvec[NB_PULSE];
Word16 dn[L_CODE];
Word16 dn2[L_CODE];
Word16 dn_sign[L_CODE];
Word16 rr[L_CODE][L_CODE];
register Word16 i;
Word16 index;
Word16 sharp;
Word16 temp;
Word32 L_temp;
L_temp = ((Word32) pitch_sharp) << 1;
/* Check for overflow condition */
if (L_temp != (Word32)((Word16) L_temp))
{
*(pOverflow) = 1;
sharp = (pitch_sharp > 0) ? MAX_16 : MIN_16;
}
else
{
sharp = (Word16) L_temp;
}
if (T0 < L_CODE)
{
for (i = T0; i < L_CODE; i++)
{
temp =
mult(
*(h + i - T0),
sharp,
pOverflow);
*(h + i) =
add(
*(h + i),
temp,
pOverflow);
}
}
cor_h_x(
h,
x,
dn,
1,
pOverflow);
/* dn2[] not used in this codebook search */
set_sign(
dn,
dn_sign,
dn2,
8);
cor_h(
h,
dn_sign,
rr,
pOverflow);
search_2i40(
subNr,
dn,
rr,
codvec,
pOverflow);
index =
build_code(
subNr,
codvec,
dn_sign,
code,
h,
y,
sign,
pOverflow);
/*-----------------------------------------------------------------*
* Compute innovation vector gain. *
* Include fixed-gain pitch contribution into code[]. *
*-----------------------------------------------------------------*/
if (T0 < L_CODE)
{
for (i = T0; i < L_CODE; i++)
{
temp =
mult(
*(code + i - T0),
sharp,
pOverflow);
*(code + i) =
add(
*(code + i),
temp,
pOverflow);
}
}
return(index);
}
Vec reflection_reflection(const Vec &x , int depth, const Sphere &sph, const Ray &r, const Vec &n, const Vec &f){
Ray rr2;
rr2.origin = x;
rr2.direction = r.direction-n*2*dot(n,r.direction);
return sph.light + mult(f,calculate_light(rr2,depth));
}
/*************************************************************************
*
* Function: gain_adapt()
* Purpose: calculate pitch/codebook gain adaptation factor alpha
* (and update the adaptor state)
*
**************************************************************************
*/
void gain_adapt(
GainAdaptState *st, /* i : state struct */
Word16 ltpg, /* i : ltp coding gain (log2()), Q13 */
Word16 gain_cod, /* i : code gain, Q1 */
Word16 *alpha /* o : gain adaptation factor, Q15 */
)
{
Word16 adapt; /* adaptdation status; 0, 1, or 2 */
Word16 result; /* alpha factor, Q13 */
Word16 filt; /* median-filtered LTP coding gain, Q13 */
Word16 tmp, i;
/* basic adaptation */
test ();
if (sub (ltpg, LTP_GAIN_THR1) <= 0)
{
adapt = 0; move16 ();
}
else
{
test ();
if (sub (ltpg, LTP_GAIN_THR2) <= 0)
{
adapt = 1; move16 ();
}
else
{
adapt = 2; move16 ();
}
}
/*
* // onset indicator
* if ((cbGain > onFact * cbGainMem[0]) && (cbGain > 100.0))
* onset = 8;
* else
* if (onset)
* onset--;
*/
/* tmp = cbGain / onFact; onFact = 2.0; 200 Q1 = 100.0 */
tmp = shr_r (gain_cod, 1);
test (); test ();
if ((sub (tmp, st->prev_gc) > 0) && sub(gain_cod, 200) > 0)
{
st->onset = 8; move16 ();
}
else
{
test ();
if (st->onset != 0)
{
st->onset = sub (st->onset, 1); move16 ();
}
}
/*
* // if onset, increase adaptor state
* if (onset && (gainAdapt < 2)) gainAdapt++;
*/
test(); test ();
if ((st->onset != 0) && (sub (adapt, 2) < 0))
{
adapt = add (adapt, 1);
}
st->ltpg_mem[0] = ltpg; move16 ();
filt = gmed_n (st->ltpg_mem, 5); move16 (); /* function result */
test ();
if (adapt == 0)
{
test ();
if (sub (filt, 5443) > 0) /* 5443 Q13 = 0.66443... */
{
result = 0; move16 ();
}
else
{
test ();
if (filt < 0)
{
result = 16384; move16 (); /* 16384 Q15 = 0.5 */
}
else
{ /* result = 0.5 - 0.75257499*filt */
/* result (Q15) = 16384 - 24660 * (filt << 2) */
filt = shl (filt, 2); /* Q15 */
result = sub (16384, mult (24660, filt));
}
}
}
else
{
result = 0; move16 ();
}
/*
* if (prevAlpha == 0.0) result = 0.5 * (result + prevAlpha);
*/
test ();
if (st->prev_alpha == 0)
{
result = shr (result, 1);
}
/* store the result */
*alpha = result; move16 ();
/* update adapter state memory */
st->prev_alpha = result; move16 ();
st->prev_gc = gain_cod; move16 ();
for (i = LTPG_MEM_SIZE-1; i > 0; i--)
{
st->ltpg_mem[i] = st->ltpg_mem[i-1]; move16 ();
}
/* mem[0] is just present for convenience in calling the gmed_n[5]
* function above. The memory depth is really LTPG_MEM_SIZE-1.
*/
}
void cbsearch(Word16 x[], /* i : target vector, Q0 */
Word16 h[], /* i : impulse response of weighted synthesis*/
/* filter h[-L_subfr..-1] must be set to */
/* zero. Q12 */
Word16 T0, /* i : Pitch lag */
Word16 pitch_sharp,/* i : Last quantized pitch gain, Q14 */
Word16 gain_pit, /* i : Pitch gain, Q14 */
Word16 res2[], /* i : Long term prediction residual, Q0 */
Word16 code[], /* o : Innovative codebook, Q13 */
Word16 y[], /* o : filtered fixed codebook excitation */
/* Q12 */
Word16 **anap, /* o : Signs of the pulses */
enum Mode mode, /* i : coder mode */
Word16 subNr, /* i : subframe number */
CommonAmrTbls* common_amr_tbls, /* ptr to struct of tables */
Flag *pOverflow) /* o : Flag set when overflow occurs */
{
Word16 index;
Word16 i;
Word16 temp;
Word16 pit_sharpTmp;
/* For MR74, the pre and post CB pitch sharpening is included in the
* codebook search routine, while for MR122 is it not.
*/
if ((mode == MR475) || (mode == MR515))
{
/* MR475, MR515 */
*(*anap)++ =
code_2i40_9bits(
subNr,
x,
h,
T0,
pitch_sharp,
code,
y,
&index,
common_amr_tbls->startPos_ptr,
pOverflow);
*(*anap)++ = index; /* sign index */
}
else if (mode == MR59)
{ /* MR59 */
*(*anap)++ =
code_2i40_11bits(
x,
h,
T0,
pitch_sharp,
code,
y,
&index,
pOverflow);
*(*anap)++ = index; /* sign index */
}
else if (mode == MR67)
{ /* MR67 */
*(*anap)++ =
code_3i40_14bits(
x,
h,
T0,
pitch_sharp,
code,
y,
&index,
pOverflow);
*(*anap)++ = index; /* sign index */
}
else if ((mode == MR74) || (mode == MR795))
{ /* MR74, MR795 */
*(*anap)++ =
code_4i40_17bits(
x,
h,
T0,
pitch_sharp,
code,
y,
&index,
common_amr_tbls->gray_ptr,
pOverflow);
*(*anap)++ = index; /* sign index */
}
else if (mode == MR102)
{ /* MR102 */
/*-------------------------------------------------------------*
* - include pitch contribution into impulse resp. h1[] *
*-------------------------------------------------------------*/
/* pit_sharpTmp = pit_sharp; */
/* if (pit_sharpTmp > 1.0) pit_sharpTmp = 1.0; */
pit_sharpTmp =
shl(
pitch_sharp,
1,
pOverflow);
for (i = T0; i < L_SUBFR; i++)
{
temp =
mult(
h[i - T0],
pit_sharpTmp,
pOverflow);
h[i] =
add_16(
h[i],
temp,
pOverflow);
}
/*--------------------------------------------------------------*
* - Innovative codebook search (find index and gain) *
*--------------------------------------------------------------*/
code_8i40_31bits(
x,
res2,
h,
code,
y,
*anap,
pOverflow);
*anap += 7;
/*-------------------------------------------------------*
* - Add the pitch contribution to code[]. *
*-------------------------------------------------------*/
for (i = T0; i < L_SUBFR; i++)
{
temp =
mult(
code[i - T0],
pit_sharpTmp,
pOverflow);
code[i] =
add_16(
code[i],
temp,
pOverflow);
}
}
else
{ /* MR122 */
/*-------------------------------------------------------------*
* - include pitch contribution into impulse resp. h1[] *
*-------------------------------------------------------------*/
/* pit_sharpTmp = gain_pit; */
/* if (pit_sharpTmp > 1.0) pit_sharpTmp = 1.0; */
pit_sharpTmp = shl(gain_pit, 1, pOverflow);
for (i = T0; i < L_SUBFR; i++)
{
temp = ((Word32)h[i - T0] * pit_sharpTmp) >> 15;
/*
mult(
h[i - T0],
,
pOverflow);
*/
h[i] =
add_16(
h[i],
temp,
pOverflow);
}
/*--------------------------------------------------------------*
* - Innovative codebook search (find index and gain) *
*--------------------------------------------------------------*/
code_10i40_35bits(
x,
res2,
h,
code,
y,
*anap,
common_amr_tbls->gray_ptr,
pOverflow);
*anap += 10;
/*-------------------------------------------------------*
* - Add the pitch contribution to code[]. *
*-------------------------------------------------------*/
for (i = T0; i < L_SUBFR; i++)
{
temp =
mult(
code[i - T0],
pit_sharpTmp,
pOverflow);
code[i] =
add_16(
code[i],
temp,
pOverflow);
}
}
}
/*************************************************************************
*
* FUNCTION: MR795_gain_code_quant3
*
* PURPOSE: Pre-quantization of codebook gains, given three possible
* LTP gains (using predicted codebook gain)
*
*************************************************************************/
void
MR795_gain_code_quant3(
Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */
Word16 gcode0, /* i : predicted CB gain (norm.), Q14 */
Word16 g_pitch_cand[], /* i : Pitch gain candidates (3), Q14 */
Word16 g_pitch_cind[], /* i : Pitch gain cand. indices (3), Q0 */
Word16 frac_coeff[], /* i : coefficients (5), Q15 */
Word16 exp_coeff[], /* i : energy coefficients (5), Q0 */
/* coefficients from calc_filt_ener()*/
Word16 *gain_pit, /* o : Pitch gain, Q14 */
Word16 *gain_pit_ind, /* o : Pitch gain index, Q0 */
Word16 *gain_cod, /* o : Code gain, Q1 */
Word16 *gain_cod_ind, /* o : Code gain index, Q0 */
Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */
/* (for MR122 MA predictor update) */
Word16 *qua_ener /* o : quantized energy error, Q10 */
/* (for other MA predictor update) */
)
{
const Word16 *p;
Word16 i, j, cod_ind, pit_ind;
Word16 e_max, exp_code;
Word16 g_pitch, g2_pitch, g_code, g2_code_h, g2_code_l;
Word16 g_pit_cod_h, g_pit_cod_l;
Word16 coeff[5], coeff_lo[5];
Word16 exp_max[5];
Word32 L_tmp, L_tmp0, dist_min;
/*
* The error energy (sum) to be minimized consists of five terms, t[0..4].
*
* t[0] = gp^2 * <y1 y1>
* t[1] = -2*gp * <xn y1>
* t[2] = gc^2 * <y2 y2>
* t[3] = -2*gc * <xn y2>
* t[4] = 2*gp*gc * <y1 y2>
*
*/
/* determine the scaling exponent for g_code: ec = ec0 - 10 */
exp_code = sub(exp_gcode0, 10);
/* calculate exp_max[i] = s[i]-1 */
exp_max[0] = sub(exp_coeff[0], 13);
exp_max[1] = sub(exp_coeff[1], 14);
exp_max[2] = add(exp_coeff[2], add(15, shl(exp_code, 1)));
exp_max[3] = add(exp_coeff[3], exp_code);
exp_max[4] = add(exp_coeff[4], add(exp_code,1));
/*-------------------------------------------------------------------*
* Find maximum exponent: *
* ~~~~~~~~~~~~~~~~~~~~~~ *
* *
* For the sum operation, all terms must have the same scaling; *
* that scaling should be low enough to prevent cgOverflow. There- *
* fore, the maximum scale is determined and all coefficients are *
* re-scaled: *
* *
* e_max = max(exp_max[i]) + 1; *
* e = exp_max[i]-e_max; e <= 0! *
* c[i] = c[i]*2^e *
*-------------------------------------------------------------------*/
e_max = exp_max[0];
for (i = 1; i < 5; i++) /* implemented flattened */
{
if (sub(exp_max[i], e_max) > 0)
{
e_max = exp_max[i];
}
}
e_max = add(e_max, 1); /* To avoid cgOverflow */
for (i = 0; i < 5; i++) {
j = sub(e_max, exp_max[i]);
L_tmp = L_deposit_h(frac_coeff[i]);
L_tmp = L_shr(L_tmp, j);
L_Extract(L_tmp, &coeff[i], &coeff_lo[i]);
}
/*-------------------------------------------------------------------*
* Codebook search: *
* ~~~~~~~~~~~~~~~~ *
* *
* For each of the candiates LTP gains in g_pitch_cand[], the terms *
* t[0..4] are calculated from the values in the table (and the *
* pitch gain candidate) and summed up; the result is the mean *
* squared error for the LPT/CB gain pair. The index for the mini- *
* mum MSE is stored and finally used to retrieve the quantized CB *
* gain *
*-------------------------------------------------------------------*/
/* start with "infinite" MSE */
dist_min = MAX_32;
cod_ind = 0;
pit_ind = 0;
/* loop through LTP gain candidates */
for (j = 0; j < 3; j++)
{
/* pre-calculate terms only dependent on pitch gain */
g_pitch = g_pitch_cand[j];
g2_pitch = mult(g_pitch, g_pitch);
L_tmp0 = Mpy_32_16( coeff[0], coeff_lo[0], g2_pitch);
L_tmp0 = Mac_32_16(L_tmp0, coeff[1], coeff_lo[1], g_pitch);
p = &qua_gain_code[0];
for (i = 0; i < NB_QUA_CODE; i++)
{
g_code = *p++; /* this is g_fac Q11 */
p++; /* skip log2(g_fac) */
p++; /* skip 20*log10(g_fac) */
g_code = mult(g_code, gcode0);
L_tmp = L_mult (g_code, g_code);
L_Extract (L_tmp, &g2_code_h, &g2_code_l);
L_tmp = L_mult(g_code, g_pitch);
L_Extract (L_tmp, &g_pit_cod_h, &g_pit_cod_l);
L_tmp = Mac_32 (L_tmp0, coeff[2], coeff_lo[2],
g2_code_h, g2_code_l);
L_tmp = Mac_32_16(L_tmp, coeff[3], coeff_lo[3],
g_code);
L_tmp = Mac_32 (L_tmp, coeff[4], coeff_lo[4],
g_pit_cod_h, g_pit_cod_l);
/* store table index if MSE for this index is lower
than the minimum MSE seen so far; also store the
pitch gain for this (so far) lowest MSE */
if (L_sub(L_tmp, dist_min) < (Word32) 0)
{
dist_min = L_tmp;
cod_ind = i;
pit_ind = j;
}
}
}
/*------------------------------------------------------------------*
* read quantized gains and new values for MA predictor memories *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
*------------------------------------------------------------------*/
/* Read the quantized gains */
p = &qua_gain_code[add (add (cod_ind, cod_ind), cod_ind)];
g_code = *p++;
*qua_ener_MR122 = *p++;
*qua_ener = *p;
/*------------------------------------------------------------------*
* calculate final fixed codebook gain: *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* gc = gc0 * g *
*------------------------------------------------------------------*/
L_tmp = L_mult(g_code, gcode0);
L_tmp = L_shr(L_tmp, sub(9, exp_gcode0));
*gain_cod = extract_h(L_tmp);
*gain_cod_ind = cod_ind;
*gain_pit = g_pitch_cand[pit_ind];
*gain_pit_ind = g_pitch_cind[pit_ind];
}
static Word16 D4i40_17_fast(/*(o) : Index of pulses positions. */
Word16 dn[], /* (i) : Correlations between h[] and Xn[]. */
Word16 rr[], /* (i) : Correlations of impulse response h[]. */
Word16 h[], /* (i) Q12: Impulse response of filters. */
Word16 cod[], /* (o) Q13: Selected algebraic codeword. */
Word16 y[], /* (o) Q12: Filtered algebraic codeword. */
Word16 *sign /* (o) : Signs of 4 pulses. */
)
{
Word16 i0, i1, i2, i3, ip0, ip1, ip2, ip3;
Word16 i, j, ix, iy, track, trk, max;
Word16 prev_i0, i1_offset;
Word16 psk, ps, ps0, ps1, ps2, sq, sq2;
Word16 alpk, alp, alp_16;
Word32 s, alp0, alp1, alp2;
Word16 *p0, *p1, *p2, *p3, *p4;
Word16 sign_dn[L_SUBFR], sign_dn_inv[L_SUBFR], *psign;
Word16 tmp_vect[NB_POS];
Word16 *rri0i0, *rri1i1, *rri2i2, *rri3i3, *rri4i4;
Word16 *rri0i1, *rri0i2, *rri0i3, *rri0i4;
Word16 *rri1i2, *rri1i3, *rri1i4;
Word16 *rri2i3, *rri2i4;
Word16 *ptr_rri0i3_i4;
Word16 *ptr_rri1i3_i4;
Word16 *ptr_rri2i3_i4;
Word16 *ptr_rri3i3_i4;
/* Init pointers */
rri0i0 = rr;
rri1i1 = rri0i0 + NB_POS;
rri2i2 = rri1i1 + NB_POS;
rri3i3 = rri2i2 + NB_POS;
rri4i4 = rri3i3 + NB_POS;
rri0i1 = rri4i4 + NB_POS;
rri0i2 = rri0i1 + MSIZE;
rri0i3 = rri0i2 + MSIZE;
rri0i4 = rri0i3 + MSIZE;
rri1i2 = rri0i4 + MSIZE;
rri1i3 = rri1i2 + MSIZE;
rri1i4 = rri1i3 + MSIZE;
rri2i3 = rri1i4 + MSIZE;
rri2i4 = rri2i3 + MSIZE;
/*-----------------------------------------------------------------------*
* Chose the sign of the impulse. *
*-----------------------------------------------------------------------*/
for (i=0; i<L_SUBFR; i++)
{
if (dn[i] >= 0)
{
sign_dn[i] = MAX_16;
sign_dn_inv[i] = MIN_16;
}
else
{
sign_dn[i] = MIN_16;
sign_dn_inv[i] = MAX_16;
dn[i] = negate(dn[i]);
}
}
/*-------------------------------------------------------------------*
* Modification of rrixiy[] to take signs into account. *
*-------------------------------------------------------------------*/
p0 = rri0i1;
p1 = rri0i2;
p2 = rri0i3;
p3 = rri0i4;
for(i0=0; i0<L_SUBFR; i0+=STEP)
{
psign = sign_dn;
if (psign[i0] < 0) psign = sign_dn_inv;
for(i1=1; i1<L_SUBFR; i1+=STEP)
{
*p0++ = mult(*p0, psign[i1]);
*p1++ = mult(*p1, psign[i1+1]);
*p2++ = mult(*p2, psign[i1+2]);
*p3++ = mult(*p3, psign[i1+3]);
}
}
p0 = rri1i2;
p1 = rri1i3;
p2 = rri1i4;
for(i1=1; i1<L_SUBFR; i1+=STEP)
{
psign = sign_dn;
if (psign[i1] < 0) psign = sign_dn_inv;
for(i2=2; i2<L_SUBFR; i2+=STEP)
{
*p0++ = mult(*p0, psign[i2]);
*p1++ = mult(*p1, psign[i2+1]);
*p2++ = mult(*p2, psign[i2+2]);
}
}
p0 = rri2i3;
p1 = rri2i4;
for(i2=2; i2<L_SUBFR; i2+=STEP)
{
psign = sign_dn;
if (psign[i2] < 0) psign = sign_dn_inv;
for(i3=3; i3<L_SUBFR; i3+=STEP)
{
*p0++ = mult(*p0, psign[i3]);
*p1++ = mult(*p1, psign[i3+1]);
}
}
/*-------------------------------------------------------------------*
* Search the optimum positions of the four pulses which maximize *
* square(correlation) / energy *
*-------------------------------------------------------------------*/
psk = -1;
alpk = 1;
ptr_rri0i3_i4 = rri0i3;
ptr_rri1i3_i4 = rri1i3;
ptr_rri2i3_i4 = rri2i3;
ptr_rri3i3_i4 = rri3i3;
/* Initializations only to remove warning from some compilers */
ip0=0; ip1=1; ip2=2; ip3=3; ix=0; iy=0; ps=0;
/* search 2 times: track 3 and 4 */
for (track=3, trk=0; track<5; track++, trk++)
{
/*------------------------------------------------------------------*
* depth first search 3, phase A: track 2 and 3/4. *
*------------------------------------------------------------------*/
sq = -1;
alp = 1;
/* i0 loop: 2 positions in track 2 */
prev_i0 = -1;
for (i=0; i<2; i++)
{
max = -1;
/* search "dn[]" maximum position in track 2 */
for (j=2; j<L_SUBFR; j+=STEP)
{
if ((sub(dn[j], max) > 0) && (sub(prev_i0,j) != 0))
{
max = dn[j];
i0 = j;
}
}
prev_i0 = i0;
j = mult(i0, 6554); /* j = i0/5 */
p0 = rri2i2 + j;
ps1 = dn[i0];
alp1 = L_mult(*p0, _1_4);
/* i1 loop: 8 positions in track 2 */
p0 = ptr_rri2i3_i4 + shl(j, 3);
p1 = ptr_rri3i3_i4;
for (i1=track; i1<L_SUBFR; i1+=STEP)
{
ps2 = add(ps1, dn[i1]); /* index increment = STEP */
/* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */
alp2 = L_mac(alp1, *p0++, _1_2);
alp2 = L_mac(alp2, *p1++, _1_4);
sq2 = mult(ps2, ps2);
alp_16 = round(alp2);
s = L_msu(L_mult(alp,sq2),sq,alp_16);
if (s > 0)
{
sq = sq2;
ps = ps2;
alp = alp_16;
ix = i0;
iy = i1;
}
}
}
i0 = ix;
i1 = iy;
i1_offset = shl(mult(i1, 6554), 3); /* j = 8*(i1/5) */
/*------------------------------------------------------------------*
* depth first search 3, phase B: track 0 and 1. *
*------------------------------------------------------------------*/
ps0 = ps;
alp0 = L_mult(alp, _1_4);
sq = -1;
alp = 1;
/* build vector for next loop to decrease complexity */
p0 = rri1i2 + mult(i0, 6554);
p1 = ptr_rri1i3_i4 + mult(i1, 6554);
p2 = rri1i1;
p3 = tmp_vect;
for (i3=1; i3<L_SUBFR; i3+=STEP)
{
/* rrv[i3] = rr[i3][i3] + rr[i0][i3] + rr[i1][i3]; */
s = L_mult(*p0, _1_4); p0 += NB_POS;
s = L_mac(s, *p1, _1_4); p1 += NB_POS;
s = L_mac(s, *p2++, _1_8);
*p3++ = round(s);
}
/* i2 loop: 8 positions in track 0 */
p0 = rri0i2 + mult(i0, 6554);
p1 = ptr_rri0i3_i4 + mult(i1, 6554);
p2 = rri0i0;
p3 = rri0i1;
for (i2=0; i2<L_SUBFR; i2+=STEP)
{
ps1 = add(ps0, dn[i2]); /* index increment = STEP */
/* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */
alp1 = L_mac(alp0, *p0, _1_8); p0 += NB_POS;
alp1 = L_mac(alp1, *p1, _1_8); p1 += NB_POS;
alp1 = L_mac(alp1, *p2++, _1_16);
/* i3 loop: 8 positions in track 1 */
p4 = tmp_vect;
for (i3=1; i3<L_SUBFR; i3+=STEP)
{
ps2 = add(ps1, dn[i3]); /* index increment = STEP */
/* alp1 = alp0 + rr[i0][i3] + rr[i1][i3] + rr[i2][i3] + 1/2*rr[i3][i3]; */
alp2 = L_mac(alp1, *p3++, _1_8);
alp2 = L_mac(alp2, *p4++, _1_2);
sq2 = mult(ps2, ps2);
alp_16 = round(alp2);
s = L_msu(L_mult(alp,sq2),sq,alp_16);
if (s > 0)
{
sq = sq2;
alp = alp_16;
ix = i2;
iy = i3;
}
}
}
/*----------------------------------------------------------------*
* depth first search 3: compare codevector with the best case. *
*----------------------------------------------------------------*/
s = L_msu(L_mult(alpk,sq),psk,alp);
if (s > 0)
{
psk = sq;
alpk = alp;
ip2 = i0;
ip3 = i1;
ip0 = ix;
ip1 = iy;
}
/*------------------------------------------------------------------*
* depth first search 4, phase A: track 3 and 0. *
*------------------------------------------------------------------*/
sq = -1;
alp = 1;
/* i0 loop: 2 positions in track 3/4 */
prev_i0 = -1;
for (i=0; i<2; i++)
{
max = -1;
/* search "dn[]" maximum position in track 3/4 */
for (j=track; j<L_SUBFR; j+=STEP)
{
if ((sub(dn[j], max) > 0) && (sub(prev_i0,j) != 0))
{
max = dn[j];
i0 = j;
}
}
prev_i0 = i0;
j = mult(i0, 6554); /* j = i0/5 */
p0 = ptr_rri3i3_i4 + j;
ps1 = dn[i0];
alp1 = L_mult(*p0, _1_4);
/* i1 loop: 8 positions in track 0 */
p0 = ptr_rri0i3_i4 + j;
p1 = rri0i0;
for (i1=0; i1<L_SUBFR; i1+=STEP)
{
ps2 = add(ps1, dn[i1]); /* index increment = STEP */
/* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */
alp2 = L_mac(alp1, *p0, _1_2); p0 += NB_POS;
alp2 = L_mac(alp2, *p1++, _1_4);
sq2 = mult(ps2, ps2);
alp_16 = round(alp2);
s = L_msu(L_mult(alp,sq2),sq,alp_16);
if (s > 0)
{
sq = sq2;
ps = ps2;
alp = alp_16;
ix = i0;
iy = i1;
}
}
}
i0 = ix;
i1 = iy;
i1_offset = shl(mult(i1, 6554), 3); /* j = 8*(i1/5) */
/*------------------------------------------------------------------*
* depth first search 4, phase B: track 1 and 2. *
*------------------------------------------------------------------*/
ps0 = ps;
alp0 = L_mult(alp, _1_4);
sq = -1;
alp = 1;
/* build vector for next loop to decrease complexity */
p0 = ptr_rri2i3_i4 + mult(i0, 6554);
p1 = rri0i2 + i1_offset;
p2 = rri2i2;
p3 = tmp_vect;
for (i3=2; i3<L_SUBFR; i3+=STEP)
{
/* rrv[i3] = rr[i3][i3] + rr[i0][i3] + rr[i1][i3]; */
s = L_mult(*p0, _1_4); p0 += NB_POS;
s = L_mac(s, *p1++, _1_4);
s = L_mac(s, *p2++, _1_8);
*p3++ = round(s);
}
/* i2 loop: 8 positions in track 1 */
p0 = ptr_rri1i3_i4 + mult(i0, 6554);
p1 = rri0i1 + i1_offset;
p2 = rri1i1;
p3 = rri1i2;
for (i2=1; i2<L_SUBFR; i2+=STEP)
{
ps1 = add(ps0, dn[i2]); /* index increment = STEP */
/* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */
alp1 = L_mac(alp0, *p0, _1_8); p0 += NB_POS;
alp1 = L_mac(alp1, *p1++, _1_8);
alp1 = L_mac(alp1, *p2++, _1_16);
/* i3 loop: 8 positions in track 2 */
p4 = tmp_vect;
for (i3=2; i3<L_SUBFR; i3+=STEP)
{
ps2 = add(ps1, dn[i3]); /* index increment = STEP */
/* alp1 = alp0 + rr[i0][i3] + rr[i1][i3] + rr[i2][i3] + 1/2*rr[i3][i3]; */
alp2 = L_mac(alp1, *p3++, _1_8);
alp2 = L_mac(alp2, *p4++, _1_2);
sq2 = mult(ps2, ps2);
alp_16 = round(alp2);
s = L_msu(L_mult(alp,sq2),sq,alp_16);
if (s > 0)
{
sq = sq2;
alp = alp_16;
ix = i2;
iy = i3;
}
}
}
/*----------------------------------------------------------------*
* depth first search 1: compare codevector with the best case. *
*----------------------------------------------------------------*/
s = L_msu(L_mult(alpk,sq),psk,alp);
if (s > 0)
{
psk = sq;
alpk = alp;
ip3 = i0;
ip0 = i1;
ip1 = ix;
ip2 = iy;
}
ptr_rri0i3_i4 = rri0i4;
ptr_rri1i3_i4 = rri1i4;
ptr_rri2i3_i4 = rri2i4;
ptr_rri3i3_i4 = rri4i4;
}
/* Set the sign of impulses */
i0 = sign_dn[ip0];
i1 = sign_dn[ip1];
i2 = sign_dn[ip2];
i3 = sign_dn[ip3];
/* Find the codeword corresponding to the selected positions */
for(i=0; i<L_SUBFR; i++) {
cod[i] = 0;
}
cod[ip0] = shr(i0, 2); /* From Q15 to Q13 */
cod[ip1] = shr(i1, 2);
cod[ip2] = shr(i2, 2);
cod[ip3] = shr(i3, 2);
/* find the filtered codeword */
for (i = 0; i < ip0; i++) y[i] = 0;
if(i0 > 0)
for(i=ip0, j=0; i<L_SUBFR; i++, j++) y[i] = h[j];
else
for(i=ip0, j=0; i<L_SUBFR; i++, j++) y[i] = negate(h[j]);
if(i1 > 0)
for(i=ip1, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]);
else
for(i=ip1, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]);
if(i2 > 0)
for(i=ip2, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]);
else
for(i=ip2, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]);
if(i3 > 0)
for(i=ip3, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]);
else
for(i=ip3, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]);
/* find codebook index; 17-bit address */
i = 0;
if(i0 > 0) i = add(i, 1);
if(i1 > 0) i = add(i, 2);
if(i2 > 0) i = add(i, 4);
if(i3 > 0) i = add(i, 8);
*sign = i;
ip0 = mult(ip0, 6554); /* ip0/5 */
ip1 = mult(ip1, 6554); /* ip1/5 */
ip2 = mult(ip2, 6554); /* ip2/5 */
i = mult(ip3, 6554); /* ip3/5 */
j = add(i, shl(i, 2)); /* j = i*5 */
j = sub(ip3, add(j, 3)); /* j= ip3%5 -3 */
ip3 = add(shl(i, 1), j);
i = add(ip0, shl(ip1, 3));
i = add(i , shl(ip2, 6));
i = add(i , shl(ip3, 9));
return i;
}
inline ivect mult(imat M, ivect v) {
return matc2vec(mult(M, vec2matc(v)), 0);
}
static int random(int seed)
{
int tmp;
tmp = (mult(seed,CONST_b)+1);
return tmp;
}
void UART2_ProcessSpektrumData( )
{
// PROCESS Spektrum receiver data -- see http://www.desertrc.com/spektrum_protocol.htm for protocol
// Determine what to do with received character
uint8_t i;
int16_t scaleInt;
_Q16 tmpQ16, scaleQ16, scale2Q16;
// Data is 14 bytes long
for( i=0; i<14; i+=2)
{
uint16_t rcdata = spektrumData[i] << 8; // MSB
rcdata += spektrumData[i+1]; // LSB
#ifdef DSMX
// 11-bit data mode
uint16_t cmddata = (int16_t) (rcdata & 0b0000011111111111); // get last 11 bits
uint8_t channel = rcdata >> 11; // get 5 first bits
scaleQ16 = num1024;
scale2Q16 = num2048;
scaleInt = 8;
#else
// 10-bit data mode
uint16_t cmddata = (int16_t) (rcdata & 0b0000001111111111); // get last 10 bits
uint8_t channel = rcdata >> 10; // get 6 first bits
scaleQ16 = num512;
scale2Q16 = num1024;
scaleInt = 4;
#endif
switch(channel){ // process channel data
case 0:
RCdata.ch0 = cmddata;
break;
case 1:
int16toQ16(&tmpQ16,&cmddata);
tmpQ16 -= scaleQ16;
RCdata.ch1 = _IQ16div(tmpQ16,scale2Q16);
break;
case 2:
int16toQ16(&tmpQ16,&cmddata);
tmpQ16 -= scaleQ16;
RCdata.ch2 = _IQ16div(tmpQ16,scale2Q16);
break;
case 3:
int16toQ16(&tmpQ16,&cmddata);
tmpQ16 -= scaleQ16;
RCdata.ch3 = _IQ16div(tmpQ16,scaleQ16);
break;
case 4:
RCdata.ch4 = cmddata;
break;
case 5:
RCdata.ch5 = cmddata;
break;
case 6:
RCdata.ch6 = cmddata;
break;
default:
break;
} // End switch channel
} // End for each data byte
// manual mode
if (RCdata.ch4 > 600) {
CmdData.AttCmd = 1;
//Debug: Scaling throttle down 2 times
//CmdData.throttle = RCdata.ch0/(scaleInt*2); // RCdata is between 0 and 1023, this will be approximately between 0 and 255
//Normal throttle
CmdData.throttle = RCdata.ch0/(scaleInt); // RCdata is between 0 and 1023, this will be approximately between 0 and 255
_Q16 halfRoll = -RCdata.ch1; //mult(RCdata.ch1,num0p5); // this is about -0.6 -> 0.6 which is +/- ~70 degrees
_Q16 halfPitch = RCdata.ch2; //mult(RCdata.ch2,num0p5);
_Q16 cosRoll = _Q16cos(halfRoll);
_Q16 sinRoll = _Q16sin(halfRoll);
_Q16 cosPitch = _Q16cos(halfPitch);
_Q16 sinPitch = _Q16sin(halfPitch);
CmdData.q_cmd.o = mult(cosRoll,cosPitch);
CmdData.q_cmd.x = mult(sinRoll,cosPitch);
CmdData.q_cmd.y = mult(cosRoll,sinPitch);
CmdData.q_cmd.z = -mult(sinRoll,sinPitch);
CmdData.p = CmdData.q = 0;
CmdData.r = mult(RCdata.ch3,num2p0);
// Turn on green LED to signify manual control mode ON
led_on(LED_GREEN);
} else
{
// Turn off green LED to signify manual control mode OFF
led_off(LED_GREEN);
CmdData.AttCmd = 0;
}
}
Node Node::operator*(const IntLeaf &leaf) const {
return mult(leaf);
}
void prim_conv11::operator()
(
signal_& quality__io,
signal_& wiregroup__io,
signal_& hstrip__io,
signal_& clctpat__io,
signal_& ph__io,
signal_& th__io,
signal_& vl__io,
signal_& phzvl__io,
signal_& me11a__io,
signal_& clctpat_r__io,
signal_& ph_hit__io,
signal_& th_hit__io,
signal_& sel__io,
signal_& addr__io,
signal_& r_in__io,
signal_& r_out__io,
signal_& we__io,
signal_& clk__io,
signal_& control_clk__io
)
{
if (!built)
{
seg_ch = 2;
bw_ph = 8;
bw_th = 7;
bw_fph = 12;
bw_fth = 8;
bw_wg = 7;
bw_ds = 7;
bw_hs = 8;
pat_w_st3 = 3;
pat_w_st1 = pat_w_st3 + 1;
full_pat_w_st3 = (1 << (pat_w_st3+1)) - 1;
full_pat_w_st1 = (1 << (pat_w_st1+1)) - 1;
padding_w_st1 = full_pat_w_st1 / 2;
padding_w_st3 = full_pat_w_st3 / 2;
red_pat_w_st3 = pat_w_st3 * 2 + 1;
red_pat_w_st1 = pat_w_st1 * 2 + 1;
fold = 4;
th_ch11 = seg_ch*seg_ch;
bw_q = 4;
bw_addr = 7;
ph_raw_w = (1 << pat_w_st3) * 15;
th_raw_w = (1 << bw_th);
max_drift = 3;
bw_phi = 12;
bw_eta = 7;
ph_hit_w = 40+4;
ph_hit_w20 = ph_hit_w;
ph_hit_w10 = 20+4;
th_hit_w = 56 + 8;
endcap = 1;
n_strips = (station <= 1 && cscid <= 2) ? 64 :
(station <= 1 && cscid >= 6) ? 64 : 80;
n_wg = (station <= 1 && cscid <= 3) ? 48 :
(station <= 1 && cscid >= 6) ? 32 :
(station == 2 && cscid <= 3) ? 112 :
(station >= 3 && cscid <= 3) ? 96 : 64;
th_coverage = (station <= 1 && cscid <= 2) ? 45 :
(station <= 1 && cscid >= 6) ? 27 :
(station <= 1 && cscid >= 3) ? 39 :
(station == 2 && cscid <= 2) ? 43 :
(station == 2 && cscid >= 3) ? 56 :
(station == 3 && cscid <= 2) ? 34 :
(station == 3 && cscid >= 3) ? 52 :
(station == 4 && cscid <= 2) ? 28 :
(station == 4 && cscid >= 3) ? 50 : 0;
ph_coverage = (station <= 1 && cscid >= 6) ? 15 : //30 :
(station >= 2 && cscid <= 2) ? 40 : 20;
th_ch = (station <= 1 && cscid <= 2) ? (seg_ch*seg_ch) : seg_ch;
ph_reverse = (endcap == 1 && station >= 3) ? 1 :
(endcap == 2 && station < 3) ? 1 : 0;
th_mem_sz = (1 << bw_addr);
th_corr_mem_sz = (1 << bw_addr);
mult_bw = bw_fph + 11;
ph_zone_bnd1 = (station <= 1 && cscid <= 2) ? 41 :
(station == 2 && cscid <= 2) ? 41 :
(station == 2 && cscid > 2) ? 87 :
(station == 3 && cscid > 2) ? 49 :
(station == 4 && cscid > 2) ? 49 : 127;
ph_zone_bnd2 = (station == 3 && cscid > 2) ? 87 : 127;
zone_overlap = 2;
bwr = 6;
bpow = 6;
cnr = (1 << bpow);
cnrex = ph_raw_w;
build();
// input parameters from MPC
quality.attach(quality__io);
wiregroup.attach(wiregroup__io);
hstrip.attach(hstrip__io);
clctpat.attach(clctpat__io);
sel.attach(sel__io);
addr.attach(addr__io);
r_in.attach(r_in__io);
we.attach(we__io);
clk.attach(clk__io);
control_clk.attach(control_clk__io);
// outputs
// low-precision ph, only for detection
// high-precision ph with displacement correction will be calculated when
// 3 best tracks are found.
ph.attach(ph__io);
// full precision th, but without displacement correction, takes th duplication into account
th.attach(th__io);
// one-bit valid flags
vl.attach(vl__io);
phzvl.attach(phzvl__io);
me11a.attach(me11a__io);
clctpat_r.attach(clctpat_r__io);
// ph and th raw hits
ph_hit.attach(ph_hit__io);
th_hit.attach(th_hit__io);
r_out.attach(r_out__io);
}
pc_id(3,0) = cscid;
pc_id(7,4) = station;
r_out = (sel == const_(2, 0x0UL)) ? params[addr] :
(sel == const_(2, 0x1UL)) ? th_mem[addr] :
(sel == const_(2, 0x2UL)) ? th_corr_mem[addr] : pc_id;
beginalways();
if (posedge (control_clk))
{
if (( (sel) == 0)) { { if (we) params [addr] = r_in; } } else
if (( (sel) == 1)) { { if (we) th_mem [addr] = r_in; } } else
if (( (sel) == 2)) { { if (we) th_corr_mem[addr] = r_in(3,0); } }
}
endalways();
beginalways();
if (posedge (clk))
{
// zero outputs
vl = 0;
phzvl = 0;
for (i = 0; i < seg_ch; i = i+1) { fph[i] = 0; clctpat_r[i] = 0; }
for (i = 0; i < th_ch; i = i+1) th[i] = 0;
ph_hit = 0;
th_hit = 0;
for (i = 0; i < seg_ch; i = i+1)
{
factor[i] = (station <= 1 && cscid <= 2 && hstrip[i] > 127) ? 1707 : // ME1/1a
1301; // ME1/1b
//if(factor[i])
// std::cout<<"factor 11 = "<<factor[i]<<std::endl;
me11a_w[i] = (station <= 1 && cscid <= 2 && hstrip[i] > 127);
if (( (clctpat[i]) == 0)) { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else
if (( (clctpat[i]) == 1)) { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else
if (( (clctpat[i]) == 2)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 1; } } else
if (( (clctpat[i]) == 3)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 0; } } else
if (( (clctpat[i]) == 4)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 1; } } else
if (( (clctpat[i]) == 5)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 0; } } else
if (( (clctpat[i]) == 6)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 1; } } else
if (( (clctpat[i]) == 7)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 0; } } else
if (( (clctpat[i]) == 8)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 1; } } else
if (( (clctpat[i]) == 9)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 0; } } else
if (( (clctpat[i]) == 10)) { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } }
// reverse clct pattern correction if chamber is reversed
// if (ph_reverse) clct_pat_sign = ~clct_pat_sign;
// convert into 1/8 strips and remove ME1/1a offset (512=128*4)
eight_str[i] = (const_s(2, 0x0UL), hstrip [i], const_s(2, 0x0UL)) - (me11a_w[i] ? 512 : 0);
// clct pattern correction
if (clct_pat_sign == 0) eight_str[i] = eight_str[i] + clct_pat_corr(2,1);
else eight_str[i] = eight_str[i] - clct_pat_corr(2,1);
if (quality[i])
{
vl[i] = 1;
// ph conversion
// for factors 1024 and 2048 the multiplier should be replaced with shifts by synthesizer
mult = eight_str[i] * factor[i];
ph_tmp = mult(mult_bw-1 , 10);
//std::cout<<"ph_tmp = "<<ph_tmp<<std::endl;
ph_init_ix = me11a_w[i] ? const_(3, 2UL) : const_(3, 0UL); // index of ph_init parameter to apply (different for ME11a and b)
//std::cout<<"ph_init_ix = "<<ph_init_ix<<std::endl;
if (ph_reverse)
{
fph[i] = params[ph_init_ix] - ph_tmp;
// set ph raw hits
ph_hit[ph_coverage - ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)] = 1;
}
else
{
fph[i] = params[ph_init_ix] + ph_tmp;
// set ph raw hits
ph_hit[ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)] = 1;
/* -----\/----- EXCLUDED -----\/-----
// add hits to take ME11a strip ganging into account
// offsets of 14 and 28 is what I observe from MC. Calculations show 11.6 and 23.2 (???)
if (me11a_w[i])
{
ph_hit[ph_tmp(bw_fph-1,5) + params[2] + 14] = 1;
ph_hit[ph_tmp(bw_fph-1,5) + params[2] + 28] = 1;
}
-----/\----- EXCLUDED -----/\----- */
}
//std::cout<<"estr = "<<eight_str[i]<<", factor = "<<factor[i]<<", ph_rev = "<<ph_reverse<<", ph_tmp = "<<ph_tmp<<std::endl;
//std::cout<<"ph_hit = "<<ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)<<std::endl;
//std::cout<<"strip = "<<hstrip[i]<<", Id = "<<cscid + 1<<",phinit = "<<(params[ph_init_ix])<<"ph_cov = "<<ph_coverage<<", and phshift = "<<(ph_tmp(bw_fph-1,5))<<", and phdisp = "<<(params[ph_init_ix + const_(3, 1UL)](7,1))<<"\n\n\n";
wg = wiregroup[i];
// th conversion
// call appropriate LUT, it returns th[i] relative to wg0 of that chamber
th_orig = th_mem[wg];
//std::cout<<"wire = "<<wg<<", th_mem[wg] = "<<th_orig<<std::endl;
// need th duplication here
for (j = 0; j < seg_ch; j = j+1)
{
if (quality[j])
{
// calculate correction for each strip number
// index is: (wiregroup(2 MS bits), dblstrip(5-bit for these chambers))
index = (wg(5,4), eight_str[j](8,4));
th_corr = th_corr_mem[index];
//std::cout<<"eightstrip = "<<eight_str[j]<<", eightstrip(8,4) = "<<eight_str[j](8,4)<<", index = "<<index<<", th_corr = "<<th_corr<<std::endl;
//std::cout<<"th_corr = "<<th_corr<<" and th_orig = "<<th_orig<<std::endl;
// apply correction to the corresponding output
if (ph_reverse) th_tmp = (th_orig - th_corr) & const_(6, 0x3fUL);
else th_tmp = (th_orig + th_corr) & const_(6, 0x3fUL);
//std::cout<<"ph_reverse = "<<ph_reverse<<" ";
//if(th_tmp)std::cout<<"th_tmp = "<<th_tmp<<" and thcoverage = "<<th_coverage<<std::endl;
// check that correction did not make invalid value outside chamber coverage
// this will actually take care of both positive and negative illegal values
if (th_tmp < th_coverage)
{
// apply initial th value for that chamber
th[i*seg_ch+j] = th_tmp + params[4];
//std::cout<<"params[4] = "<<params[4]<<"\n";
// th hits
th_hit[th_tmp + params[5]] = 1;
// check which zones ph hits should be applied to
if (th[i*seg_ch+j] <= (ph_zone_bnd1 + zone_overlap)) phzvl[0] = 1;
if (th[i*seg_ch+j] > (ph_zone_bnd2 - zone_overlap)) phzvl[2] = 1;
if (
(th[i*seg_ch+j] > (ph_zone_bnd1 - zone_overlap)) &&
(th[i*seg_ch+j] <= (ph_zone_bnd2 + zone_overlap))
) phzvl[1] = 1;
//std::cout<<"ph_zone_bnd1 = "<<ph_zone_bnd1<<std::endl;
//std::cout<<"phzvl = "<<phzvl<<std::endl;
}
}
}
clctpat_r[i] = clctpat[i]; // just propagate pattern downstream
} // if (quality[i])
ph[i] = fph[i];
//if(fph[i]) std::cout<<"fph["<<i<<"] = "<<fph[i]<<" and vl[i] = "<<vl[i]<<std::endl;
} // for (i = 0; i < seg_ch; i = i+1)
me11a = me11a_w;
}
endalways();
}
void WebRtcG729fix_Decod_ld8a(
Decod_ld8a_state *st,
int16_t parm[], /* (i) : vector of synthesis parameters
parm[0] = bad frame indicator (bfi) */
int16_t synth[], /* (o) : synthesis speech */
int16_t A_t[], /* (o) : decoded LP filter in 2 subframes */
int16_t *T2, /* (o) : decoded pitch lag in 2 subframes */
int16_t *Vad, /* (o) : frame type */
int16_t bad_lsf /* (i) : bad LSF indicator */
)
{
int16_t *Az; /* Pointer on A_t */
int16_t lsp_new[M]; /* LSPs */
int16_t code[L_SUBFR]; /* ACELP codevector */
/* Scalars */
int16_t i, j, i_subfr;
int16_t T0, T0_frac, index;
int16_t bfi;
int32_t L_temp;
int16_t bad_pitch; /* bad pitch indicator */
/* for G.729B */
int16_t ftyp;
int16_t lsfq_mem[MA_NP][M];
int Overflow;
/* Test bad frame indicator (bfi) */
bfi = *parm++;
/* for G.729B */
ftyp = *parm;
if(bfi == 1) {
if(st->past_ftyp == 1) {
ftyp = 1;
parm[4] = 1; /* G.729 maintenance */
}
else ftyp = 0;
*parm = ftyp; /* modification introduced in version V1.3 */
}
*Vad = ftyp;
/* Processing non active frames (SID & not transmitted) */
if(ftyp != 1) {
WebRtcG729fix_Get_decfreq_prev(st, lsfq_mem);
WebRtcG729fix_Dec_cng(st, st->past_ftyp, st->sid_sav, st->sh_sid_sav,
parm, st->exc, st->lsp_old, A_t, &st->seed, lsfq_mem);
WebRtcG729fix_Update_decfreq_prev(st, lsfq_mem);
Az = A_t;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
Overflow = WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 0);
if(Overflow != 0) {
/* In case of overflow in the synthesis */
/* -> Scale down vector exc[] and redo synthesis */
for (i = 0; i < PIT_MAX + L_INTERPOL + L_FRAME; i++)
st->old_exc[i] = shr(st->old_exc[i], 2);
WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 1);
}
else
WEBRTC_SPL_MEMCPY_W16(st->mem_syn, &synth[i_subfr+L_SUBFR-M], M);
Az += MP1;
*T2++ = st->old_T0;
}
st->sharp = SHARPMIN;
}
/* Processing active frame */
else {
st->seed = INIT_SEED;
parm++;
/* Decode the LSPs */
WebRtcG729fix_D_lsp(st, parm, lsp_new, WebRtcSpl_AddSatW16(bfi, bad_lsf));
parm += 2;
/*
Note: "bad_lsf" is introduce in case the standard is used with
channel protection.
*/
/* Interpolation of LPC for the 2 subframes */
WebRtcG729fix_Int_qlpc(st->lsp_old, lsp_new, A_t);
/* update the LSFs for the next frame */
WEBRTC_SPL_MEMCPY_W16(st->lsp_old, lsp_new, M);
/*------------------------------------------------------------------------*
* Loop for every subframe in the analysis frame *
*------------------------------------------------------------------------*
* The subframe size is L_SUBFR and the loop is repeated L_FRAME/L_SUBFR *
* times *
* - decode the pitch delay *
* - decode algebraic code *
* - decode pitch and codebook gains *
* - find the excitation and compute synthesis speech *
*------------------------------------------------------------------------*/
Az = A_t; /* pointer to interpolated LPC parameters */
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR)
{
index = *parm++; /* pitch index */
if(i_subfr == 0)
{
i = *parm++; /* get parity check result */
bad_pitch = WebRtcSpl_AddSatW16(bfi, i);
if( bad_pitch == 0)
{
WebRtcG729fix_Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
st->old_T0 = T0;
}
else /* Bad frame, or parity error */
{
T0 = st->old_T0;
T0_frac = 0;
st->old_T0 = WebRtcSpl_AddSatW16(st->old_T0, 1);
if(st->old_T0 > PIT_MAX) {
st->old_T0 = PIT_MAX;
}
}
}
else /* second subframe */
{
if(bfi == 0)
{
WebRtcG729fix_Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac);
st->old_T0 = T0;
}
else
{
T0 = st->old_T0;
T0_frac = 0;
st->old_T0 = WebRtcSpl_AddSatW16(st->old_T0, 1);
if (st->old_T0 > PIT_MAX) {
st->old_T0 = PIT_MAX;
}
}
}
*T2++ = T0;
/*-------------------------------------------------*
* - Find the adaptive codebook vector. *
*-------------------------------------------------*/
WebRtcG729fix_Pred_lt_3(&st->exc[i_subfr], T0, T0_frac, L_SUBFR);
/*-------------------------------------------------------*
* - Decode innovative codebook. *
* - Add the fixed-gain pitch contribution to code[]. *
*-------------------------------------------------------*/
if(bfi != 0) /* Bad frame */
{
parm[0] = WebRtcG729fix_Random(&st->seed_fer) & (int16_t)0x1fff; /* 13 bits random */
parm[1] = WebRtcG729fix_Random(&st->seed_fer) & (int16_t)0x000f; /* 4 bits random */
}
WebRtcG729fix_Decod_ACELP(parm[1], parm[0], code);
parm +=2;
j = shl(st->sharp, 1); /* From Q14 to Q15 */
if(T0 < L_SUBFR) {
for (i = T0; i < L_SUBFR; i++) {
code[i] = WebRtcSpl_AddSatW16(code[i], mult(code[i-T0], j));
}
}
/*-------------------------------------------------*
* - Decode pitch and codebook gains. *
*-------------------------------------------------*/
index = *parm++; /* index of energy VQ */
WebRtcG729fix_Dec_gain(st, index, code, L_SUBFR, bfi, &st->gain_pitch, &st->gain_code);
/*-------------------------------------------------------------*
* - Update pitch sharpening "sharp" with quantized gain_pitch *
*-------------------------------------------------------------*/
st->sharp = st->gain_pitch;
if (st->sharp > SHARPMAX) { st->sharp = SHARPMAX; }
else if (st->sharp < SHARPMIN) { st->sharp = SHARPMIN; }
/*-------------------------------------------------------*
* - Find the total excitation. *
* - Find synthesis speech corresponding to exc[]. *
*-------------------------------------------------------*/
for (i = 0; i < L_SUBFR; i++)
{
/* exc[i] = gain_pitch*exc[i] + gain_code*code[i]; */
/* exc[i] in Q0 gain_pitch in Q14 */
/* code[i] in Q13 gain_codeode in Q1 */
L_temp = L_mult(st->exc[i+i_subfr], st->gain_pitch);
L_temp = L_mac(L_temp, code[i], st->gain_code);
L_temp = L_shl(L_temp, 1);
st->exc[i+i_subfr] = L_round(L_temp);
}
Overflow = WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 0);
if(Overflow != 0)
{
/* In case of overflow in the synthesis */
/* -> Scale down vector exc[] and redo synthesis */
for (i = 0; i < PIT_MAX + L_INTERPOL + L_FRAME; i++)
st->old_exc[i] = shr(st->old_exc[i], 2);
WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 1);
}
else
WEBRTC_SPL_MEMCPY_W16(st->mem_syn, &synth[i_subfr+L_SUBFR-M], M);
Az += MP1; /* interpolated LPC parameters for next subframe */
}
}
/*------------*
* For G729b
*-----------*/
if(bfi == 0) {
L_temp = 0L;
for (i = 0; i < L_FRAME; i++) {
L_temp = L_mac(L_temp, st->exc[i], st->exc[i]);
} /* may overflow => last level of SID quantizer */
st->sh_sid_sav = WebRtcSpl_NormW32(L_temp);
st->sid_sav = L_round(L_shl(L_temp, st->sh_sid_sav));
st->sh_sid_sav = WebRtcSpl_SubSatW16(16, st->sh_sid_sav);
}
/*--------------------------------------------------*
* Update signal for next frame. *
* -> shift to the left by L_FRAME exc[] *
*--------------------------------------------------*/
Copy(&st->old_exc[L_FRAME], &st->old_exc[0], PIT_MAX+L_INTERPOL);
/* for G729b */
st->past_ftyp = ftyp;
return;
}