int16_t sqrts(int16_t x)
{
int16_t xb, y, exp, idx, bv_sub_frac, bv_sub_tab;
int32_t a0;
if (x <= 0) {
y = 0;
} else {
exp = bv_norm_s(x);
/* use 65-entry table */
xb = bv_shl(x, exp); // normalization of x
idx = bv_shr(xb, 9); // for 65 entry table
a0 = bv_L_deposit_h(tabsqrt[idx]); // Q31 table look-up value
bv_sub_frac = bv_shl((int16_t) (xb & 0x01FF), 6); // Q15 bv_sub-fraction
bv_sub_tab = bv_sub(tabsqrt[idx + 1], tabsqrt[idx]); // Q15 table interval for interpolation
a0 = bv_L_mac(a0, bv_sub_frac, bv_sub_tab); // Q31 linear interpolation between table entries
if (exp & 0x0001) {
exp = bv_shr(bv_add(exp, 1), 1); // normalization of sqrt()
a0 = L_bv_shr(a0, exp);
y = intround(a0); // Q15
a0 = bv_L_mac(a0, 13573, y); // Q31 incorporate the missing "/sqrt(2)"
} else {
exp = bv_shr(exp, 1); // normalization of sqrt()
a0 = L_bv_shr(a0, exp); // Q31
}
y = intround(a0); // Q15
}
return y;
}
Word16 sqrts(Word16 x)
{
Word16 xb, y, exp, idx, sub_frac, sub_tab;
Word32 a0;
if(x <= 0){
y = 0;
}
else{
exp = norm_s(x);
/* use 65-entry table */
xb = shl(x, exp); // normalization of x
idx = shr(xb, 9); // for 65 entry table
a0 = L_deposit_h(tabsqrt[idx]); // Q31 table look-up value
sub_frac = shl((Word16)(xb & 0x01FF), 6); // Q15 sub-fraction
sub_tab = sub(tabsqrt[idx+1], tabsqrt[idx]); // Q15 table interval for interpolation
a0 = L_mac(a0, sub_frac, sub_tab); // Q31 linear interpolation between table entries
if(exp & 0x0001){
exp = shr(add(exp, 1), 1); // normalization of sqrt()
a0 = L_shr(a0, exp);
y = intround(a0); // Q15
a0 = L_mac(a0, 13573, y); // Q31 incorporate the missing "/sqrt(2)"
}
else{
exp = shr(exp, 1); // normalization of sqrt()
a0 = L_shr(a0, exp); // Q31
}
y = intround(a0); // Q15
}
return y;
}
void Log2(int32_t x, /* (i) input */
int16_t * int_comp, /* Q0 integer part */
int16_t * frac_comp /* Q15 fractional part */
)
{
int16_t exp, idx_man, bv_sub_man, bv_sub_tab;
int32_t a0;
if (x <= 0) {
*int_comp = 0;
*frac_comp = 0;
} else {
exp = bv_norm_l(x); // normalization
a0 = L_bv_shl(x, exp); // Q30 mantissa, i.e. 1.xxx Q30
/* use table look-up of man in [1.0, 2.0[ Q30 */
a0 = L_bv_shr(L_bv_sub(a0, (int32_t) 0x40000000), 8); // Q16 index into table - note zero'ing of leading 1
idx_man = bv_extract_h(a0); // Q0 index into table
bv_sub_man = bv_extract_l(L_bv_shr((a0 & 0xFFFF), 1)); // Q15 fractional bv_sub_man
a0 = bv_L_deposit_h(tablog[idx_man]); // Q31
bv_sub_tab = bv_sub(tablog[idx_man + 1], tablog[idx_man]); // Q15
a0 = bv_L_mac(a0, bv_sub_man, bv_sub_tab); // Q31
*frac_comp = intround(a0); // Q15
*int_comp = bv_sub(30, exp); // Q0
}
return;
}
void get_pq_polynomials(
Word32 *f, /* Q23 */
Word16 *lsp) /* Q15 */
{
Word16 i, n, hi, lo;
Word16 index, offset, coslsp, c;
Word32 a0;
f[0] = L_mult(2048, 2048); // 1.0 Q23
for(i = 1; i <= LPCO ; i++)
f[i]= 0;
for(n=1; n<=(LPCO>>1); n++) {
/* cosine mapping */
index = shr(lsp[2*n-2],9); // Q6
offset = lsp[2*n-2]&(Word16)0x01ff; // Q9
a0 = L_mult(sub(costable[index+1], costable[index]), offset); // Q10
coslsp = add(costable[index], intround(L_shl(a0, 6))); // Q15 cos((double)PI*lsp[2*n-2])
c = coslsp; // Q14 c = 2. * cos((double)PI*lsp[2*n-2])
for(i = 2*n; i >= 2; i--) {
L_Extract(f[i-1], &hi, &lo);
f[i] = L_add(f[i], f[i-2]); // Q23 f[i] += f[i-2]
a0 = Mpy_32_16(hi, lo, c); // Q22
f[i] = L_sub(f[i], L_shl(a0,1)); // Q23 f[i] += f[i-2] - c*f[i-1];
}
f[1] = L_msu(f[1], c, 256); // Q23 f[1] -= c;
}
return;
}
static void mct_ClickAutotemp(uint8_t line, volatile long &pos, bool &adjustValue, uint8_t which)
{
float *temp = NULL;
float scale = 1;
switch (which) {
case AUTOTEMP_MIN:
temp = &autotemp_min;
break;
case AUTOTEMP_MAX:
temp = &autotemp_max;
break;
case AUTOTEMP_FACTOR:
temp = &autotemp_factor;
scale = 100;
break;
default:
return;
}
adjustValue = !adjustValue;
if (adjustValue) {
pos = intround(*temp * scale);
} else {
*temp = pos / scale;
}
}
void Log2(
Word32 x, /* (i) input */
Word16 *int_comp, /* Q0 integer part */
Word16 *frac_comp /* Q15 fractional part */
)
{
Word16 exp, idx_man, sub_man, sub_tab;
Word32 a0;
if(x <= 0){
*int_comp = 0;
*frac_comp = 0;
}
else{
exp = norm_l(x); // normalization
a0 = L_shl(x, exp); // Q30 mantissa, i.e. 1.xxx Q30
/* use table look-up of man in [1.0, 2.0[ Q30 */
a0 = L_shr(L_sub(a0, (Word32)0x40000000), 8); // Q16 index into table - note zero'ing of leading 1
idx_man = extract_h(a0); // Q0 index into table
sub_man = extract_l(L_shr((a0 & 0xFFFF), 1)); // Q15 fractional sub_man
a0 = L_deposit_h(tablog[idx_man]); // Q31
sub_tab = sub(tablog[idx_man+1], tablog[idx_man]); // Q15
a0 = L_mac(a0, sub_man, sub_tab); // Q31
*frac_comp = intround(a0); // Q15
*int_comp = sub(30, exp); // Q0
}
return;
}
void azfilter(
Word16 a[], /* (i) Q12 : prediction coefficients */
Word16 m, /* (i) : LPC order */
Word16 x[], /* (i) Q0 : input signal samples, incl. past */
Word16 y[], /* (o) Q0 : filtered output signal */
Word16 lg /* (i) : size of filtering */
)
{
Word16 i, n;
Word32 a0;
Word16 *fp1;
/* loop through every element of the current vector */
for (n = 0; n < lg; n++) {
/* perform multiply-adds along the delay line of filter */
fp1 = x + n;
a0 = L_mult0(a[0], *fp1--); // Q12
for (i = 1; i <= m; i++)
a0 = L_mac0(a0, a[i], *fp1--); // Q12
/* get the output with rounding */
y[n] = intround(L_shl(a0, 4)); // Q0
}
return;
}
void mct_ClickNozzle(uint8_t line, volatile long &pos, bool &adjustValue, uint8_t extruder)
{
adjustValue = !adjustValue;
if (adjustValue) {
pos = intround(degTargetHotend(extruder));
} else {
setTargetHotend(pos, extruder);
}
}
void mct_ClickBed(uint8_t line, volatile long &pos, bool &adjustValue, uint8_t which)
{
adjustValue = !adjustValue;
if (adjustValue) {
pos = intround(degTargetBed());
} else {
setTargetBed(pos);
}
}
Word16 FNevChebP(
Word16 x, /* (i) Q15: value */
Word16 *t_man, /* (i) Q7: mantissa of coefficients */
Word16 *t_exp, /* (i): exponent fo cofficients */
Word16 nd2) /* (i): order */
{
Word16 i;
Word16 x2;
Word16 b_man[NAB], b_exp[NAB];
Word16 y;
Word32 a0;
x2 = x; // 2x in Q14
b_man[0] = t_man[nd2];
b_exp[0] = t_exp[nd2]; // b[0] in Q(7+t_exp)
a0 = L_mult(x2, b_man[0]);
a0 = L_shr(a0, sub(b_exp[0], 1)); // t*b[0] in Q23
a0 = L_add(a0, L_shr(L_deposit_h(t_man[nd2-1]), t_exp[nd2-1])); // c[nd2-1] + t*b[0] in Q23
b_exp[1] = norm_l(a0);
b_man[1] = intround(L_shl(a0, b_exp[1])); // b[1] = c[nd2-1] + t * b[0]
for (i=2;i<nd2;i++){
a0 = L_mult(x2, b_man[i-1]);
a0 = L_shr(a0, sub(b_exp[i-1], 1)); // t*b[i-1] in Q23
a0 = L_add(a0, L_shr(L_deposit_h(t_man[nd2-i]), t_exp[nd2-i]));// c[nd2-i] + t*b[i-1] in Q23
a0 = L_sub(a0, L_shr(L_deposit_h(b_man[i-2]), b_exp[i-2])); // c[nd2-i] + t*b[i-1] - b[i-2] in Q23
b_exp[i] = norm_l(a0);
b_man[i] = intround(L_shl(a0, b_exp[i])); // b[i] = c[nd2-i] - b[i-2] + t * b[i-1]
}
a0 = L_mult(x, b_man[nd2-1]);
a0 = L_shr(a0, b_exp[nd2-1]); // x*b[nd2-1] in Q23
a0 = L_add(a0, L_shr(L_deposit_h(t_man[0]), t_exp[0])); // c[0] + x*b[nd2-1] in Q23
a0 = L_sub(a0, L_shr(L_deposit_h(b_man[nd2-2]), b_exp[nd2-2])); // c[0] + x*b[nd2-1] - b[nd2-2] in Q23
y = intround(L_shl(a0, 6)); // Q13
return y; // Q13
}
void lsp2a(
Word16 lsp[], /* (i) Q15 : line spectral pairs */
Word16 a[]) /* (o) Q12 : predictor coefficients (order = 10) */
{
Word32 p[LPCO+1], q[LPCO+1]; // Q23
Word32 a0;
Word16 i, n;
get_pq_polynomials(p, lsp);
get_pq_polynomials(q, lsp+1);
a[0] = 4096; // Q12
a0 = L_add(p[1], q[1]); // Q23
a[1] = intround(L_shl(a0,4)); // Q12 - includes 0.5 factor of a[1] = 0.5*(p[1]+q[1])
for(i=1, n=2; i<LPCO; i++, n++) {
a0 = L_add(p[i], p[n]); // Q23
a0 = L_add(a0, q[n]); // Q23
a0 = L_sub(a0, q[i]); // Q23
a[n] = intround(L_shl(a0,4)); // Q12 a[n] = 0.5 * (p[i] + p[n] + q[n] - q[i]);
}
return;
}
void sqrt_i(Word16 x_man, Word16 x_exp, Word16 *y_man, Word16 *y_exp)
{
Word16 x_manb, x_expb, y, exp, idx, sub_frac, sub_tab;
Word32 a0;
if(x_man <= 0){
*y_man = 0;
*y_exp = 0;
}
else{
exp = norm_s(x_man);
x_manb = shl(x_man, exp); // normalize to Q15 in [0; 1] for table look-up
x_expb = add(x_exp, exp); // update exponent for x (left-shifting by additionl exp)
x_expb = sub(x_expb, 15); // we need to take sqrt of 0-32767 number but table is for 0-1
idx = shr(x_manb, 9); // for 65 entry table
a0 = L_deposit_h(tabsqrt[idx]); // Q31 table look-up value
sub_frac = shl((Word16)(x_manb & 0x01FF), 6);// Q15 sub-fraction
sub_tab = sub(tabsqrt[idx+1], tabsqrt[idx]); // Q15 table interval for interpolation
a0 = L_mac(a0, sub_frac, sub_tab); // Q31 linear interpolation between table entries
exp = norm_l(a0); // exponent of a0
y = intround(L_shl(a0,exp)); // normalize sqrt-root and drop 16 LBSs
exp = add(15, exp); // exponent of a0 taking Q15 of y into account
if(x_expb & 0x0001){
if(y < 0x5A82){ // 1*sqrt(2) in Q14) - with div_s(y1, y2), y2 must be >= y1
exp = add(exp, shr(add(x_expb,1), 1)); // normalization for sqrt()
*y_man = div_s(0x2D41, y); // 0x2D41 is 1/sqrt(2) in Q14
}
else{
exp = add(exp, shr(sub(x_expb,1), 1)); // normalization for sqrt()
*y_man = div_s(0x5A82, y); // 0x5A82 is 1*sqrt(2) in Q14
}
*y_exp = sub(29, exp); // ...and div_s returns fraction divide in Q15
}
else{
exp = add(exp, shr(x_expb, 1)); // normalization for sqrt()
*y_man = div_s(0x4000, y);
*y_exp = sub(29, exp); // 0x4000 is 1 in Q14 and div_s returns fraction divide in Q15
}
}
return;
}
void roundICcoords(ICdevSym &Sym, unsigned long PinEntryNum, unsigned long stepsize)
{
int tmp,i;
for(i=0;i<PinEntryNum;i++){
Sym.Pins[i].posx = intround(Sym.Pins[i].posx, stepsize);
Sym.Pins[i].posy = intround(Sym.Pins[i].posy, stepsize);
}
// Größe des Rechtecks runden
Sym.width = intround(Sym.width, stepsize);
Sym.height = intround(Sym.height, stepsize);
// Koordinaten des ersten Eckpunktes runden
// -> Bauteilmitte ist dann nicht auf Koordinatenursprung,
// wenn Höhe oder Breite ungerade Schrittanzahl haben
Sym.x1 = intround(Sym.x1, stepsize);
Sym.y1 = intround(Sym.y1, stepsize);
}
void apfilter(int16_t a[], /* (i) Q12 : prediction coefficients */
int16_t m, /* (i) : LPC order */
int16_t x[], /* (i) Q0 : input signal */
int16_t y[], /* (o) Q0 : output signal */
int16_t lg, /* (i) : size of filtering */
int16_t mem[], /* (i/o) Q0: filter memory */
int16_t update /* (i) : memory update flag */
)
{
int16_t buf[BUFFERSIZE]; /* buffer for filter memory & signal */
int32_t a0;
int16_t *fp1;
int16_t i, n;
/* copy filter memory to beginning part of temporary buffer */
W16copy(buf, mem, m);
/* loop through every element of the current vector */
for (n = 0; n < lg; n++) {
/* perform bv_multiply-bv_adds along the delay line of filter */
fp1 = &buf[n];
a0 = L_bv_mult0(4096, x[n]); // Q12
for (i = m; i > 0; i--)
a0 = bv_L_msu0(a0, a[i], *fp1++); // Q12
/* update temporary buffer for filter memory */
*fp1 = intround(L_bv_shl(a0, 4));
}
/* copy to output array */
W16copy(y, buf + m, lg);
/* get the filter memory after filtering the current vector */
if (update)
W16copy(mem, buf + lg, m);
return;
}
void BV16_Decode(
struct BV16_Bit_Stream *bs,
struct BV16_Decoder_State *ds,
Word16 *x)
{
Word32 lgq, lg_el;
Word16 gainq; /* Q3 */
Word16 pp;
Word32 a0;
Word16 gain_exp;
Word16 i;
Word16 a0hi, a0lo;
Word16 ltsym[LTMOFF+FRSZ];
Word16 xq[LXQ];
Word16 a[LPCO+1];
Word16 lspq[LPCO]; /* Q15 */
Word16 cbs[VDIM*CBSZ];
Word16 bq[3]; /* Q15 */
Word32 bss;
Word32 E;
/* set frame erasure flags */
if (ds->cfecount != 0) {
ds->ngfae = 1;
} else {
ds->ngfae++;
if (ds->ngfae>LGPORDER) ds->ngfae=LGPORDER+1;
}
/* reset frame erasure counter */
ds->cfecount = 0;
/* decode pitch period */
pp = (bs->ppidx + MINPP);
/* decode spectral information */
lspdec(lspq,bs->lspidx,ds->lsppm,ds->lsplast);
lsp2a(lspq,a);
W16copy(ds->lsplast, lspq, LPCO);
/* decode pitch taps */
pp3dec(bs->bqidx, bq);
/* decode gain */
a0 = gaindec(&lgq,bs->gidx,ds->lgpm,ds->prevlg,ds->level,
&ds->nggalgc,&lg_el);
/* gain normalization */
gain_exp = sub(norm_l(a0), 2);
/* scale down quantized gain by 1.5, 1/1.5=2/3 (21845 Q15) */
L_Extract(a0, &a0hi, &a0lo);
a0 = Mpy_32_16(a0hi, a0lo, 21845);
gainq = intround(L_shl(a0, gain_exp));
/* scale the scalar quantizer codebook to current signal level */
for (i=0;i<(VDIM*CBSZ);i++) cbs[i] = mult_r(gainq, cccb[i]);
/* copy state memory to buffer */
W16copy(xq, ds->xq, XQOFF);
W16copy(ltsym, ds->ltsym, LTMOFF);
/* decoding of the excitation signal with integrated long-term */
/* and short-term synthesis */
excdec_w_synth(xq+XQOFF,ltsym+LTMOFF,ds->stsym,bs->qvidx,bq,cbs,pp,
a,gain_exp,&E);
ds->E = E;
/* update the remaining state memory */
W16copy(ds->ltsym, ltsym+FRSZ, LTMOFF);
W16copy(ds->xq, xq+FRSZ, XQOFF);
ds->pp_last = pp;
W16copy(ds->bq_last, bq, 3);
/* level estimation */
estlevel(lg_el,&ds->level,&ds->lmax,&ds->lmin,&ds->lmean,&ds->x1,
ds->ngfae, ds->nggalgc,&ds->estl_alpha_min);
/* adaptive postfiltering */
postfilter(xq, pp, &(ds->ma_a), ds->b_prv, &(ds->pp_prv), x);
/* scale signal up by 1.5 */
for(i=0; i<FRSZ; i++)
x[i] = add(x[i], shr(x[i],1));
W16copy(ds->atplc, a, LPCO+1);
bss = L_add(L_add(bq[0], bq[1]), bq[2]);
if (bss > 32768)
bss = 32768;
else if (bss < 0)
bss = 0;
ds->per = add(shr(ds->per, 1), (Word16)L_shr(bss, 1));
}
Word16 coarsepitch(
Word16 *xw, /* (i) (normalized) weighted signal */
struct BV16_Encoder_State *cstate) /* (i/o) Coder State */
{
Word16 s; /* Q2 */
Word16 a, b;
Word16 im;
Word16 maxdev, flag, mpflag;
Word32 eni, deltae;
Word32 cc;
Word16 ah,al, bh, bl;
Word32 a0, a1, a2, a3;
Word32 *lp0;
Word16 exp, new_exp;
Word16 *fp0, *fp1, *fp2, *fp3, *sp;
Word16 *fp1_h, *fp1_l, *fp2_h, *fp2_l;
Word16 cor2max, cor2max_exp;
Word16 cor2m, cor2m_exp;
Word16 s0, t0, t1, exp0, exp1, e2, e3;
Word16 threshold;
Word16 mplth; /* Q2 */
Word16 i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1];
Word16 cpp;
Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1];
Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD];
Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */
Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD];
Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1];
Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD];
Word16 energym_man, energym_exp;
Word16 energymax_man, energymax_exp;
/* Lowpass filter xw() to 800 hz; shift & output into xwd() */
/* AP and AZ filtering and decimation */
fp1_h = tmp_h + DFO;
fp1_l = tmp_l + DFO;
sp = xw;
a1 = 1;
for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1];
for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i];
lp0 = lxwd;
for (i=0;i<FRSZD;i++) {
for (k=0;k<DECF;k++) {
a0 = L_shr(L_deposit_h(*sp++),10);
fp2_h = fp1_h-1;
fp2_l = fp1_l-1;
for (j=0;j<DFO;j++)
a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1]));
a0 = L_shl(a0, 2); /* adf Q13 */
L_Extract(a0, fp1_h++, fp1_l++);
}
fp2_h = fp1_h-1;
fp2_l = fp1_l-1;
a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]);
for (j=0;j<DFO;j++)
a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1]));
*lp0++ = a0;
a0 = L_abs(a0);
if (a1 < a0)
a1 = a0;
}
/* copy temp buffer to memory */
fp1_h -= DFO;
fp1_l -= DFO;
for (i=0;i<DFO;i++) {
cstate->dfm_h[2*i+1] = fp1_h[i];
cstate->dfm_h[2*i] = fp1_l[i];
}
lp0 = lxwd;
new_exp = sub(norm_l(a1), 3); /* headroom to avoid overflow */
exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */
if (exp < 0) { /* Descending signal level */
new_exp = cstate->xwd_exp;
exp = 0;
}
for (i=0;i<XDOFF;i++)
xwd[i] = shr(cstate->xwd[i], exp);
/* fill-in new exponent */
fp0 = xwd + XDOFF;
for (i=0;i<FRSZD;i++)
fp0[i] = intround(L_shl(lp0[i],new_exp));
/* update signal memory for next frame */
exp0 = 1;
for (i=0;i<XDOFF;i++) {
exp1 = abs_s(xwd[FRSZD+i]);
if (exp1 > exp0)
exp0 = exp1;
}
exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */
exp = sub(exp0, exp);
if (exp >=0)
{
for (i=0;i<XDOFF-FRSZD;i++)
cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp);
}
else
{
exp = -exp;
if (exp >=15)
exp = 15;
for (i=0;i<XDOFF-FRSZD;i++)
cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp);
}
for (;i<XDOFF;i++)
cstate->xwd[i] = shl(xwd[FRSZD+i],exp0);
cstate->xwd_exp = add(new_exp, exp0);
/* Compute correlation & energy of prediction basis vector */
/* reset local buffers */
for (i=0;i<MAXPPD1;i++)
cor[i] = energy[i] = 0;
fp0 = xwd+MAXPPD1;
fp1 = xwd+MAXPPD1-M1;
a0 = a1 = 0;
for (i=0;i<(LXD-MAXPPD1);i++) {
a0 = L_mac0(a0, *fp1, *fp1);
a1 = L_mac0(a1, *fp0++, *fp1++);
}
cor[M1-1] = a1;
energy[M1-1] = a0;
energy_exp[M1-1] = norm_l(energy[M1-1]);
energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1]));
s0 = cor2_exp[M1-1] = norm_l(a1);
t0 = extract_h(L_shl(a1, s0));
cor2[M1-1] = extract_h(L_mult(t0, t0));
if (a1 < 0)
cor2[M1-1] = negate(cor2[M1-1]);
fp2 = xwd+LXD-M1-1;
fp3 = xwd+MAXPPD1-M1-1;
for (i=M1;i<M2;i++) {
fp0 = xwd+MAXPPD1;
fp1 = xwd+MAXPPD1-i-1;
a1 = 0;
for (j=0;j<(LXD-MAXPPD1);j++)
a1 = L_mac0(a1,*fp0++,*fp1++);
cor[i] = a1;
a0 = L_msu0(a0, *fp2, *fp2);
a0 = L_mac0(a0, *fp3, *fp3);
fp2--; fp3--;
energy[i] = a0;
energy_exp[i] = norm_l(energy[i]);
energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i]));
s0 = cor2_exp[i] = norm_l(a1);
t0 = extract_h(L_shl(a1, s0));
cor2[i] = extract_h(L_mult(t0, t0));
if (a1 < 0)
cor2[i] = negate(cor2[i]);
}
/* Find positive correlation peaks */
/* Find maximum of cor*cor/energy among positive correlation peaks */
npeaks = 0;
n = MINPPD-1;
while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) {
if (cor[n]>0) {
a0 = L_mult(energy_man[n-1],cor2[n]);
a1 = L_mult(energy_man[n], cor2[n-1]);
exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1);
exp0 = add(exp0, energy_exp[n-1]);
exp0 = sub(exp0, energy_exp[n]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
a0 = L_mult(energy_man[n+1],cor2[n]);
a1 = L_mult(energy_man[n], cor2[n+1]);
exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1);
exp0 = add(exp0, energy_exp[n+1]);
exp0 = sub(exp0, energy_exp[n]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
idx[npeaks] = n;
npeaks++;
}
}
}
n++;
}
/* Return early if there is no peak or only one peak */
if (npeaks == 0){ /* if there are no positive peak, */
return MINPPD*DECF; /* return minimum pitch period in decimated domain */
}
if (npeaks == 1){ /* if there is exactly one peak, */
return (idx[0]+1)*DECF; /* return the time lag for this single peak */
}
/* If program proceeds to here, there are 2 or more peaks */
cor2max=(Word16) 0x8000;
cor2max_exp= (Word16) 0;
energymax_man=1;
energymax_exp=0;
imax=0;
for (i=0; i < npeaks; i++) {
/* Use quadratic interpolation to find the interpolated cor[] and
energy[] corresponding to interpolated peak of cor2[]/energy[] */
/* first calculate coefficients of y(x)=ax^2+bx+c; */
n=idx[i];
a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]);
L_Extract(a0, &ah, &al);
a0=L_shr(L_sub(cor[n+1],cor[n-1]),1);
L_Extract(a0, &bh, &bl);
cc=cor[n];
/* Initialize variables before searching for interpolated peak */
im=0;
cor2m_exp = cor2_exp[n];
cor2m = cor2[n];
energym_exp = energy_exp[n];
energym_man = energy_man[n];
eni=energy[n];
/* Determine which side the interpolated peak falls in, then
do the search in the appropriate range */
a0 = L_mult(energy_man[n-1],cor2[n+1]);
a1 = L_mult(energy_man[n+1], cor2[n-1]);
exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1);
exp0 = add(exp0, energy_exp[n-1]);
exp0 = sub(exp0, energy_exp[n+1]);
if (exp0>=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) { /* if right side */
deltae = L_shr(L_sub(energy[n+1], eni), 2);
for (k = 0; k < HDECF; k++) {
a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
eni = L_add(eni, deltae);
a1 = eni;
exp0 = norm_l(a0);
s0 = extract_h(L_shl(a0, exp0));
s0 = extract_h(L_mult(s0, s0));
e2 = energym_exp;
t0 = energym_man;
a2 = L_mult(t0, s0);
e3 = norm_l(a1);
t1 = extract_h(L_shl(a1, e3));
a3 = L_mult(t1, cor2m);
exp1 = shl(sub(exp0, cor2m_exp),1);
exp1 = add(exp1, e2);
exp1 = sub(exp1, e3);
if (exp1>=0)
a2 = L_shr(a2, exp1);
else
a3 = L_shl(a3, exp1);
if (a2 > a3) {
im = k+1;
cor2m = s0;
cor2m_exp = exp0;
energym_exp = e3;
energym_man = t1;
}
}
} else { /* if interpolated peak is on the left side */
deltae = L_shr(L_sub(energy[n-1], eni), 2);
for (k = 0; k < HDECF; k++) {
a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
eni = L_add(eni, deltae);
a1=eni;
exp0 = norm_l(a0);
s0 = extract_h(L_shl(a0, exp0));
s0 = extract_h(L_mult(s0, s0));
e2 = energym_exp;
t0 = energym_man;
a2 = L_mult(t0, s0);
e3 = norm_l(a1);
t1 = extract_h(L_shl(a1, e3));
a3 = L_mult(t1, cor2m);
exp1 = shl(sub(exp0, cor2m_exp),1);
exp1 = add(exp1, e2);
exp1 = sub(exp1, e3);
if (exp1>=0)
a2 = L_shr(a2, exp1);
else
a3 = L_shl(a3, exp1);
if (a2 > a3) {
im = -k-1;
cor2m = s0;
cor2m_exp = exp0;
energym_exp = e3;
energym_man = t1;
}
}
}
/* Search done; assign cor2[] and energy[] corresponding to
interpolated peak */
plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */
cor2i[i]=cor2m;
cor2i_exp[i]=cor2m_exp;
/* interpolated energy[] of i-th interpolated peak */
energyi_exp[i] = energym_exp;
energyi_man[i] = energym_man;
/* Search for global maximum of interpolated cor2[]/energy[] peak */
a0 = L_mult(cor2m,energymax_man);
a1 = L_mult(cor2max, energyi_man[i]);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
imax=i;
cor2max=cor2m;
cor2max_exp=cor2m_exp;
energymax_exp = energyi_exp[i];
energymax_man = energyi_man[i];
}
}
cpp=plag[imax]; /* first candidate for coarse pitch period */
mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */
/* Find the largest peak (if there is any) around the last pitch */
maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */
im = -1;
cor2m=(Word16) 0x8000;
cor2m_exp= (Word16) 0;
energym_man = 1;
energym_exp = 0;
for (i=0;i<npeaks;i++) { /* loop thru the peaks before the largest peak */
if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) {
a0 = L_mult(cor2i[i],energym_man);
a1 = L_mult(cor2m, energyi_man[i]);
exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1);
exp0 = add(exp0, energym_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
im=i;
cor2m=cor2i[i];
cor2m_exp=cor2i_exp[i];
energym_man = energyi_man[i];
energym_exp = energyi_exp[i];
}
}
} /* if there is no peaks around last pitch, then im is still -1 */
/* Now see if we should pick any alternatice peak */
/* first, search first half of pitch range, see if any qualified peak
has large enough peaks at every multiple of its lag */
i=0;
while (2*plag[i] < mplth) {
/* Determine the appropriate threshold for this peak */
if (i != im) { /* if not around last pitch, */
threshold = TH1; /* use a higher threshold */
} else { /* if around last pitch */
threshold = TH2; /* use a lower threshold */
}
/* If threshold exceeded, test peaks at multiples of this lag */
a0 = L_mult(cor2i[i],energymax_man);
t1 = extract_h(L_mult(energyi_man[i], threshold));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[i]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
flag=1;
j=i+1;
k=0;
s=shl(plag[i],1); /* initialize t to twice the current lag */
while (s<=mplth) { /* loop thru all multiple lag <= mplth */
mpflag=0; /* initialize multiple pitch flag to 0 */
t0 = mult_r(s,MPDTH);
a=sub(s, t0); /* multiple pitch range lower bound */
b=add(s, t0); /* multiple pitch range upper bound */
while (j < npeaks) { /* loop thru peaks with larger lags */
if (plag[j] > b) { /* if range exceeded, */
break; /* break the innermost while loop */
} /* if didn't break, then plag[j] <= b */
if (plag[j] > a) { /* if current peak lag within range, */
/* then check if peak value large enough */
a0 = L_mult(cor2i[j],energymax_man);
if (k<4)
t1 = MPTH[k];
else
t1 = MPTH4;
t1 = extract_h(L_mult(t1, energyi_man[j]));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energyi_exp[j]);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
mpflag=1; /* if peak large enough, set mpflag, */
break; /* and break the innermost while loop */
}
}
j++;
}
/* if no qualified peak found at this multiple lag */
if (mpflag == 0) {
flag=0; /* disqualify the lag plag[i] */
break; /* and break the while (s<=mplth) loop */
}
k++;
s = add(s, plag[i]); /* update s to the next multiple pitch lag */
}
/* if there is a qualified peak at every multiple of plag[i], */
if (flag == 1) {
cpp = plag[i]; /* then accept this as final pitch */
return cpp; /* and return to calling function */
}
}
i++;
if (i == npeaks)
break; /* to avoid out of array bound error */
}
/* If program proceeds to here, none of the peaks with lags < 0.5*mplth
qualifies as the final pitch. in this case, check if
there is any peak large enough around last pitch. if so, use its
lag as the final pitch. */
if (im != -1) { /* if there is at least one peak around last pitch */
if (im == imax) { /* if this peak is also the global maximum, */
return cpp; /* return first pitch candidate at global max */
}
if (im < imax) { /* if lag of this peak < lag of global max, */
a0 = L_mult(cor2m,energymax_man);
t1 = extract_h(L_mult(energym_man, LPTH2));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energym_exp);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
if (plag[im] > HMAXPPD*DECF) {
cpp=plag[im];
return cpp;
}
for (k=2; k<=5;k++) { /* check if current candidate pitch */
s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */
t0 = mult_r(s,SMDTH);
a=sub(s, t0); /* the time lag of */
b=add(s, t0); /* the global maximum peak */
if (plag[im]>a && plag[im]<b) { /* if so, */
cpp=plag[im]; /* accept this peak, */
return cpp; /* and return as pitch */
}
}
}
} else { /* if lag of this peak > lag of global max, */
a0 = L_mult(cor2m,energymax_man);
t1 = extract_h(L_mult(energym_man, LPTH1));
a1 = L_mult(cor2max, t1);
exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
exp0 = add(exp0, energymax_exp);
exp0 = sub(exp0, energym_exp);
if (exp0 >=0)
a0 = L_shr(a0, exp0);
else
a1 = L_shl(a1, exp0);
if (a0 > a1) {
cpp = plag[im]; /* if this peak is large enough, */
return cpp; /* accept its lag */
}
}
}
/* If program proceeds to here, we have no choice but to accept the
lag of the global maximum */
return cpp;
}
void excdec_w_synth(
Word16 *xq, /* (o) Q0 quantized signal vector */
Word16 *ltsym, /* (i/o) Q0 quantized excitation signal vector */
Word16 *stsym, /* (i/o) Q0 short-term predictor memory */
Word16 *idx, /* (o) quantizer codebook index for uq[] vector */
Word16 *b, /* (i) Q15 coefficient of 3-tap pitch predictor */
Word16 *cb, /* (i) Q0 codebook */
Word16 pp, /* pitch period (# of 8 kHz samples) */
Word16 *aq, /* (i) Q12 short-term predictor coefficients */
Word16 gain_exp, /* gain_exp of current sub-frame */
Word32 *EE
)
{
Word16 i, n, m, *ip, id;
Word16 *fp1, *fp2, *fp3;
Word32 a0;
Word16 tt;
Word32 E;
Word32 a1;
Word16 buf1[LPCO+FRSZ]; /* buffer for filter memory & signal */
Word16 uq[VDIM]; /* selected codebook vector (incl. sign) */
W16copy(buf1, stsym, LPCO); /* buffer is used to avoid memory shifts */
ip=idx;
E = 0;
/* Loop through every vector of the current subframe */
for (m = 0; m < FRSZ; m += VDIM) {
/********************************************************************************/
/* Excitation vector */
/********************************************************************************/
id = *ip++; /* get codebook index of current vector */
fp1 = uq;
if (id < CBSZ){
fp2 = &cb[id*VDIM];
for (n=0;n<VDIM;n++) {
*fp1++ = *fp2++; // Q0
}
}
else {
id -= CBSZ;
fp2 = &cb[id*VDIM];
for (n=0;n<VDIM;n++) {
*fp1++ = negate(*fp2++); // Q0
}
}
/********************************************************************************/
/* Long-term and short-term synthesis */
/********************************************************************************/
fp2 = uq;
fp3 = ltsym + m;
for (n = m; n < m + VDIM; n++) {
/* Un-normalized excitation */
a0 = L_shr(L_deposit_h(*fp2++), gain_exp); // Q16
/* Excitation energy for PLC */
tt = intround(a0); // Q0
E = L_mac0(E, tt, tt);
/* Long-term predicion */
fp1 = <sym[n-pp+1]; // Q0
a1 = L_mult(*fp1--, b[0]); // Q16
a1 = L_mac(a1, *fp1--, b[1]);
a1 = L_mac(a1, *fp1--, b[2]);
/* Update long-term filter synthesis memory */
a0 = L_add(a0, a1);
*fp3++ = intround(a0); // Q0
/* Short-term prediction */
fp1 = &buf1[n]; // Q0
a1 = 0; // Q13
for(i = LPCO; i > 0; i--)
a1 = L_msu(a1, *fp1++, aq[i]); // Q13
a1 = L_shl(a1, 3); // Q16
a1 = L_add(a0, a1);
*fp1++ = intround(a1); // Q0
}
}
/* Update noise feedback filter memory after filtering current subframe */
W16copy(stsym, buf1+FRSZ, LPCO);
/* copy to speech buffer */
W16copy(xq, buf1+LPCO, FRSZ);
*EE = E;
return;
}
void a2lsp(
Word16 pc[], /* (i) Q12: predictor coefficients */
Word16 lsp[], /* (o) Q15: line spectral pairs */
Word16 old_lsp[]) /* (i) Q15: old lsp */
{
Word16 i, j, exp;
Word16 fa_man[NAB], fa_exp[NAB], fb_man[NAB], fb_exp[NAB];
Word16 ta_man[NAB], ta_exp[NAB], tb_man[NAB], tb_exp[NAB];
Word16 *t_man, *t_exp;
Word32 a0;
Word16 nd2, nf, ngrd;
Word16 xroot, xlow, ylow, ind, xhigh, yhigh, xmid, ymid, dx, dy, dxdy, x, sign;
/* Find normalization for fa and fb */
/* fb[0] = fa[0] = 1.0; */
/* for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) { */
/* fa[i] = pc[i] + pc[j] - fa[i-1]; */
/* fb[i] = pc[i] - pc[j] + fb[i-1]; */
/* } */
fa_man[0] = 16384;
fa_exp[0] = 6; // fa_man[0] in high 16-bits >> fa_exp[0] = 1.0 in Q24
fb_man[0] = 16384;
fb_exp[0] = 6; // fb_man[0] in high 16-bits >> fb_exp[0] = 1.0 in Q24
for (i = 1, j = LPCO; i <= (LPCO/2); i++, j--) {
a0 = L_mult0(pc[i], 4096); // Q24
a0 = L_mac0(a0, pc[j], 4096); // Q24
a0 = L_sub(a0, L_shr(L_deposit_h(fa_man[i-1]),fa_exp[i-1])); // Q24
fa_exp[i] = norm_l(a0);
fa_man[i] = intround(L_shl(a0, fa_exp[i])); // Q(8+fb_exp[i])
a0 = L_mult0(pc[i], 4096); // Q24
a0 = L_msu0(a0, pc[j], 4096); // Q24
a0 = L_add(a0, L_shr(L_deposit_h(fb_man[i-1]),fb_exp[i-1])); // Q24
fb_exp[i] = norm_l(a0);
fb_man[i] = intround(L_shl(a0, fb_exp[i])); // Q(8+fb_exp[i])
}
nd2 = (LPCO)/2;
/* ta[] and tb[] in Q(7+exp) */
/* ta[0] = fa[nab-1]; ta[i] = 2.0 * fa[j]; */
/* tb[0] = fb[nab-1]; tb[i] = 2.0 * fb[j]; */
ta_man[0] = fa_man[NAB-1];
ta_exp[0] = add(fa_exp[NAB-1], 1);
tb_man[0] = fb_man[NAB-1];
tb_exp[0] = add(fb_exp[NAB-1], 1);
for (i = 1, j = NAB - 2; i < NAB; ++i, --j) {
ta_man[i] = fa_man[j];
ta_exp[i] = fa_exp[j];
tb_man[i] = fb_man[j];
tb_exp[i] = fb_exp[j];
}
nf = 0;
t_man = ta_man;
t_exp = ta_exp;
xroot = 0x7fff;
ngrd = 0;
xlow = grid[0]; // Q15
ylow = FNevChebP(xlow, t_man, t_exp, nd2);
ind = 0;
/* Root search loop */
while (ngrd<(Ngrd-1) && nf < LPCO) {
ngrd++;
xhigh = xlow;
yhigh = ylow;
xlow = grid[ngrd];
ylow = FNevChebP(xlow, t_man, t_exp, nd2);
if ( L_mult(ylow ,yhigh) <= 0) {
/* Bisections of the interval containing a sign change */
dx = xhigh - xlow;
for (i = 1; i <= NBIS; ++i) {
dx = shr(dx, 1);
xmid = add(xlow, dx);
ymid = FNevChebP(xmid, t_man, t_exp, nd2);
if (L_mult(ylow,ymid) <= 0) {
yhigh = ymid;
xhigh = xmid;
} else {
ylow = ymid;
xlow = xmid;
}
}
/*
* Linear interpolation in the subinterval with a sign change
* (take care if yhigh=ylow=0)
*/
dx = sub(xhigh, xlow);
dy = sub(ylow, yhigh);
if (dy != 0) {
sign = dy;
dy = abs_s(dy);
exp = norm_s(dy);
dy = shl(dy, exp);
/* The maximum grid distance is 1629 => */
/* Maximum dx=1629/2^4=101.8125, i.e. 16384/101.8125=160.92~128 (7 bits) */
/* However, due to the starting point for the search of a new root, */
/* xlow = xroot, 1 more bit of headroom for the division is required. */
dxdy = div_s(shl(dx,6), dy);
a0 = L_mult(dxdy, ylow);
a0 = L_shr(a0, sub(6, exp));
x = intround(a0);
if(sign < 0) x = negate(x);
xmid = add(xlow, x);
}
else {
xmid = add(xlow, shr(dx,1));
}
/* acos mapping for New lsp component */
while (( costable[ind] >= xmid ) && (ind < 63)) ind++;
ind--;
a0 = L_mult( sub(xmid, costable[ind]) , acosslope[ind] );
x = intround(L_shl(a0, 4));
lsp[nf] = add(x, shl(ind, 9));
++nf;
/* Start the search for the roots of next polynomial at the estimated
* location of the root just found. We have to catch the case that the
* two polynomials have roots at the same place to avoid getting stuck at
* that root.
*/
if (xmid >= xroot) xmid = xlow - dx;
xroot = xmid;
if (t_man == ta_man){
t_man = tb_man;
t_exp = tb_exp;
}
else{
t_man = ta_man;
t_exp = ta_exp;
}
xlow = xmid;
ylow = FNevChebP(xlow, t_man, t_exp, nd2);
}
}
/* Check if all LSPs are found */
if( sub(nf, LPCO) < 0)
{
W16copy(lsp, old_lsp, LPCO);
}
return;
}
/* Standard Long-Term Postfilter */
void postfilter(
Word16 *s, /* input : quantized speech signal */
Word16 pp, /* input : pitch period */
Word16 *ma_a,
Word16 *b_prv,
Word16 *pp_prv,
Word16 *e) /* output: enhanced speech signal */
{
int n;
Word16 len, t0, t1, t2, t3, shift, aa, R0norm, R0_exp;
Word32 a0, a1, R0, R1, R01, R01max, Rx;
Word16 *fp1;
Word16 ppt, pptmin, pptmax, ppnew;
Word16 bb[2];
Word16 R1max_exp, R1max, R01Sqmax_exp, R01Sqmax, R01Sq_exp, R01Sq, R1_exp, R1n;
Word16 gainn, Rx_exp;
Word16 buf[MAXPP+FRSZ];
Word16 *ps, ww1, ww2;
Word32 step, delta;
Word16 bi0, bi1c, bi1p;
ps = s+XQOFF;
/********************************************************************/
/* pitch search around decoded pitch */
/********************************************************************/
pptmin = sub(pp, DPPQNS);
pptmax = add(pp, DPPQNS);
if (pptmin<MINPP)
{
pptmin = MINPP;
pptmax = add(pptmin, 2*DPPQNS);
}
else if (pptmax>MAXPP)
{
pptmax = MAXPP;
pptmin = sub(pptmax, 2*DPPQNS);
}
fp1 = &s[XQOFF-pptmax];
len = add(FRSZ, pptmax);
a0 = 0;
for (n=0;n<len;n++)
{
t1 = shr(*fp1++, 3);
a0 = L_mac0(a0,t1,t1);
}
shift = norm_l(a0);
if (a0==0) shift=31;
shift = sub(6, shift);
if (shift > 0)
{
ps = buf+pptmax;
fp1 = &s[XQOFF-pptmax];
shift = shr(add(shift, 1), 1);
for (n=0;n<len;n++)
{
buf[n] = shr(fp1[n], shift);
}
}
else shift=0;
R0 = 0;
R1 = 0;
R01 = 0;
for(n=0; n<FRSZ; n++)
{
R0 = L_mac0(R0, ps[n], ps[n]);
R1 = L_mac0(R1, ps[n-pptmin], ps[n-pptmin]);
R01 = L_mac0(R01,ps[n], ps[n-pptmin]);
}
R0_exp = norm_l(R0);
R0norm = extract_h(L_shl(R0, R0_exp));
R0_exp = R0_exp-16;
ppnew = pptmin;
R1max_exp = norm_l(R1);
R1max = extract_h(L_shl(R1, R1max_exp));
R01Sqmax_exp = norm_l(R01);
t1 = extract_h(L_shl(R01, R01Sqmax_exp));
R01Sqmax_exp = shl(R01Sqmax_exp, 1);
R01Sqmax = extract_h(L_mult(t1, t1));
R01max = R01;
for(ppt=pptmin+1; ppt<=pptmax; ppt++)
{
R1 = L_msu0(R1,ps[FRSZ-ppt], ps[FRSZ-ppt]);
R1 = L_mac0(R1,ps[-ppt], ps[-ppt]);
R01= 0;
for(n=0; n<FRSZ; n++)
{
R01 = L_mac0(R01, ps[n], ps[n-ppt]);
}
R01Sq_exp = norm_l(R01);
t1 = extract_h(L_shl(R01, R01Sq_exp));
R01Sq_exp = shl(R01Sq_exp, 1);
R01Sq = extract_h(L_mult(t1, t1));
R1_exp = norm_l(R1);
R1n = extract_h(L_shl(R1, R1_exp));
a0 = L_mult(R01Sq, R1max);
a1 = L_mult(R01Sqmax, R1n);
t1 = add(R01Sq_exp, R1max_exp);
t2 = add(R01Sqmax_exp, R1_exp);
t2 = sub(t1, t2);
if (t2>=0) a0 = L_shr(a0, t2);
if (t2<0) a1 = L_shl(a1, t2);
if (L_sub(a0, a1)>0)
{
R01Sqmax = R01Sq;
R01Sqmax_exp = R01Sq_exp;
R1max = R1n; R1max_exp = R1_exp;
ppnew = ppt;
R01max = R01;
}
}
/******************************************************************/
/* calculate all-zero pitch postfilter */
/******************************************************************/
if (R1max==0 || R0==0 || R01max <= 0)
{
aa = 0;
}
else
{
a0 = R1max_exp-16;
t1 = mult(R1max, R0norm);
a0 = a0+R0_exp-15;
sqrt_i(t1, (Word16)a0, &t1, &t2);
t0 = norm_l(R01max);
t3 = extract_h(L_shl(R01max, t0));
t0 = t0-16;
aa = mult(t3, t1);
t0 = t0+t2-15;
t0 = t0-15;
if (t0<0) aa = shl(aa, sub(0,t0));
else aa = shr(aa, t0);
}
a0 = L_mult(8192, aa);
a0 = L_mac(a0, 24576, *ma_a);
*ma_a = intround(a0);
if((*ma_a < ATHLD1) && (aa < (ATHLD2)))
aa = 0;
bb[1] = mult(ScLTPF, aa);
/******************************************************************/
/* calculate normalization energies */
/******************************************************************/
Rx = 0;
R0 = 0;
for(n=0; n<FRSZ; n++)
{
a0 = L_shl(s[XQOFF+n], 15);
a0 = L_add(a0, L_mult0(bb[1], s[XQOFF+n-ppnew]));
e[n] = intround(a0);
t1 = shr(e[n], shift);
t2 = shr(s[XQOFF+n], shift);
Rx = L_mac0(Rx, t1, t1);
R0 = L_mac0(R0, t2, t2);
}
R0 = L_shr(R0, 2);
if(R0 == 0 || Rx == 0)
gainn = 32767;
else
{
Rx_exp = norm_l(Rx);
t1 = extract_h(L_shl(Rx, Rx_exp));
t2 = extract_h(L_shl(R0, Rx_exp));
if (t2>= t1)
gainn = 32767;
else
{
t1 = div_s(t2, t1);
gainn = sqrts(t1);
}
}
/******************************************************************/
/* interpolate from the previous postfilter to the current */
/******************************************************************/
bb[0] = gainn;
bb[1] = mult(gainn, bb[1]);
step = (Word32)((1.0/(NINT+1))*(2147483648.0));
delta = 0;
for(n=0; n<NINT; n++)
{
delta = L_add(delta, step);
ww1 = intround(delta);
ww2 = add(sub(32767, ww1), 1);
/* interpolate between two filters */
bi0 = intround(L_mac(L_mult(ww1, bb[0]), ww2, b_prv[0]));
bi1c= mult(ww1, bb[1]);
bi1p= mult(ww2, b_prv[1]);
e[n] = intround(L_mac(L_mac(L_mult(bi1c, s[XQOFF+n-ppnew]), bi1p, s[XQOFF+n-(*pp_prv)]), bi0, s[XQOFF+n]));
}
for(n=NINT; n<FRSZ; n++)
{
e[n] = intround(L_shl(L_mult(gainn, e[n]),1));
}
/******************************************************************/
/* save state memory */
/******************************************************************/
*pp_prv = ppnew;
b_prv[0] = bb[0];
b_prv[1] = bb[1];
return;
}
void mct_ShowBed(uint8_t line, uint8_t which)
{
mct_Show(line, ftostr3(intround(degTargetBed())));
}
void mct_ShowNozzle(uint8_t line, uint8_t extruder)
{
mct_Show(line, itostr3(intround(degTargetHotend(extruder))));
}
void Levinson(int32_t r32[], /* (i) : r32[] double precision vector of autocorrelation coefficients */
int16_t a[], /* (o) : a[] in Q12 - LPC coefficients */
int16_t old_a[], /* (i/o): old_a[] in Q12 - previous LPC coefficients */
int16_t m /* (i) : LPC order */
)
{
int16_t i, j, high, low, alpha_hi, alpha_lo, alpha_exp;
int16_t exp, r_hi[LPCO + 1], r_lo[LPCO + 1];
int16_t a_hi[LPCO + 1], a_lo[LPCO + 1], anew_hi[LPCO + 1],
anew_lo[LPCO + 1];
int16_t rc_hi, rc_lo;
int32_t a0, a1, alpha_man;
memzero(r_hi, (LPCO + 1) * sizeof(int16_t));
memzero(r_lo, (LPCO + 1) * sizeof(int16_t));
memzero(anew_hi, (LPCO + 1) * sizeof(int16_t));
memzero(anew_lo, (LPCO + 1) * sizeof(int16_t));
/* Normalization of autocorrelation coefficients */
exp = bv_norm_l(r32[0]);
for (i = 0; i <= m; i++) {
r32[i] = L_bv_shl(r32[i], exp);
L_Extract(r32[i], r_hi + i, r_lo + i);
}
/* a[1] = rc = -r[1]/r[0] */
a1 = bv_L_abs(r32[1]);
a0 = Div_32(a1, r_hi[0], r_lo[0]); // rc in Q31
if (r32[1] > 0)
a0 = L_bv_negate(a0);
L_Extract(L_bv_shr(a0, 4), a_hi + 1, a_lo + 1); // Q27
/* alpha = r[0]*(1-rc*rc) */
L_Extract(a0, &high, &low);
a0 = Mpy_32(high, low, high, low); // rc^2 in Q31
a0 = bv_L_abs(a0); // Lesson from G.729
a0 = L_bv_sub(0x40000000, L_bv_shr(a0, 1)); // 1-rc*rc in Q30
L_Extract(a0, &high, &low);
a0 = Mpy_32(r_hi[0], r_lo[0], high, low); // alpha in Q30
alpha_exp = bv_norm_l(a0);
alpha_man = L_bv_shl(a0, alpha_exp);
alpha_exp = bv_sub(alpha_exp, 1); // alpha: Q(31+alpha_exp)
/* Recursive solution of Yule-Walker equations */
for (i = 2; i <= m; i++) {
/* s = r[i] + sum{r[j]*a[i-j], j=1,2,...,i-1} */
a0 = 0;
for (j = 1; j < i; j++) {
a1 = Mpy_32(r_hi[j], r_lo[j], a_hi[i - j], a_lo[i - j]); // Q27
a0 = L_bv_add(a0, a1); // Q27
}
a0 = L_bv_shl(a0, 4); // Q31
a0 = L_bv_add(a0, r32[i]); // Q31
/* rc = -s/alpha */
exp = bv_norm_l(a0);
a0 = L_bv_shl(a0, exp);
a1 = bv_L_abs(a0);
if (L_bv_sub(a1, alpha_man) >= 0) {
a1 = L_bv_shr(a1, 1);
exp = bv_sub(exp, 1);
}
L_Extract(alpha_man, &alpha_hi, &alpha_lo);
a1 = Div_32(a1, alpha_hi, alpha_lo);
if (a0 > 0)
a1 = L_bv_negate(a1); // rc in Q(31+exp-alpha_exp)
a1 = L_bv_shr(a1, bv_sub(exp, alpha_exp)); // rc in Q31
L_Extract(a1, &rc_hi, &rc_lo); // rc in Q31
/* Check for absolute value of reflection coefficient - stability */
if (bv_sub(bv_abs_s(intround(a1)), 32750) > 0) {
a[0] = 4096;
for (j = 1; j <= m; j++)
a[j] = old_a[j];
return;
}
/* anew[j]=a[j]+rc*a[i-j], j=1,2,...i-1 */
/* anew[i]=rc */
for (j = 1; j < i; j++) {
a0 = Mpy_32(a_hi[i - j], a_lo[i - j], rc_hi, rc_lo); // Q27
a0 = L_bv_add(a0, L_Comp(a_hi[j], a_lo[j])); // Q27
L_Extract(a0, anew_hi + j, anew_lo + j); // Q27
}
L_Extract(L_bv_shr(a1, 4), anew_hi + i, anew_lo + i); // Q27
/* alpha = alpha*(1-rc*rc) */
a0 = Mpy_32(rc_hi, rc_lo, rc_hi, rc_lo); // rc*rc in Q31
a0 = bv_L_abs(a0); // Lesson from G.729
a0 = L_bv_shr(a0, 1); // rc*rc in Q30
a0 = L_bv_sub(0x40000000, a0); // 1-rc*rc in Q30
L_Extract(a0, &high, &low);
a0 = Mpy_32(alpha_hi, alpha_lo, high, low); // Q(30+alpha_exp)
exp = bv_norm_l(a0);
alpha_man = L_bv_shl(a0, exp); // Q(30+exp+alpha_exp)
alpha_exp = bv_sub(bv_add(alpha_exp, exp), 1); // alpha: Q(31+alpha_exp)
/* a[j] = anew[j] in Q(12+a1_exp) */
for (j = 1; j <= i; j++) {
a_hi[j] = anew_hi[j];
a_lo[j] = anew_lo[j];
}
}
/* convert lpc coefficients to Q12 and save new lpc as old lpc for next frame */
a[0] = 4096;
for (j = 1; j <= m; j++) {
a[j] = intround(L_bv_shl(L_Comp(a_hi[j], a_lo[j]), 1)); // Q12
old_a[j] = a[j];
}
return;
}
Word16 pitchtapquan(
Word16 *x,
Word16 pp,
Word16 *b, /* Q15 */
Word32 *re) /* Q3 */
{
Word32 cormax, cor;
Word16 s0, s1, s2;
Word32 t0, t1, t2;
Word16 *xt, *sp0, *sp1, *sp2;
int ppm2, qidx=0, i, j;
Word32 p[9];
Word16 sp[9];
ppm2 = pp-2;
xt = x + XOFF;
for (i=0;i<3;i++)
{
sp0 = xt;
sp1 = x+XOFF-ppm2-i-1;
t0 = 1;
for (j=0;j<FRSZ;j++)
t0 = L_mac(t0, *sp0++, *sp1++);
p[i] = t0;
}
sp0 = x+XOFF-ppm2-3;
s0 = *sp0++;
s1 = *sp0++;
s2 = *sp0--;
t0 = L_mult(s0,s0);
p[8] = t0;
t1 = L_mult(s0,s1);
p[4] = t1;
p[5] = L_mult(s0,s2);
s0 = *sp0++;
s1 = *sp0++;
s2 = *sp0--;
t2 = L_mult(s0,s0);
p[8] = L_add(p[8],t2);
p[4] = L_mac(p[4], s0, s1);
p[5] = L_mac(p[5], s0, s2);
for (i=0;i<(FRSZ-2);i++)
{
s0 = *sp0++;
s1 = *sp0++;
s2 = *sp0--;
p[8] = L_mac(p[8], s0, s0);
p[4] = L_mac(p[4], s0, s1);
p[5] = L_mac(p[5], s0, s2);
}
s0 = *sp0++;
s1 = *sp0++;
s2 = *sp0--;
p[7] = L_mac(L_sub(p[8], t0), s0, s0);
p[3] = L_mac(L_sub(p[4], t1), s0, s1);
p[6] = L_mac(L_sub(p[7], t2), s1, s1);
s2 = 32;
for (i=0;i<9;i++)
{
if (p[i]!=0)
{
s0 = norm_l(p[i]);
if (s0 < s2) s2 = s0;
}
}
s2 = sub(s2, 2);
for (i=0;i<9;i++)
sp[i] = extract_h(L_shl(p[i],s2));
cormax = MIN_32;
sp0 = pp9cb; /* Q14 */
for (i=0;i<PPCBSZ;i++)
{
cor = 0;
sp1 = sp;
for (j=0;j<9;j++)
cor = L_mac(cor, *sp0++, *sp1++);
if (cor > cormax)
{
cormax = cor;
qidx = i;
}
}
sp2 = pp9cb + qidx*9;
for (i=0;i<3;i++)
b[i] = sp2[i]; /* multiplied by 0.5 : Q14 -> Q15 */
sp0 = x + XOFF;
sp1 = x + XOFF - ppm2 - 1;
t1 = 0;
for (i=0;i<FRSZ;i++)
{
t0 = L_deposit_h(*sp0++);
t0 = L_msu(t0, b[0], sp1[0]);
t0 = L_msu(t0, b[1], sp1[-1]);
t0 = L_msu(t0, b[2], sp1[-2]);
s0 = intround(t0); /* Q1 use as the pitch prediction residual */
sp1++;
t1 = L_mac(t1, s0, s0);
}
*re = t1;
return qidx;
}
