/*
**
** Function: Calc_Exc_Rand()
**
** Description: Computation of random excitation for inactive frames:
** Adaptive codebook entry selected randomly
** Higher rate innovation pattern selected randomly
** Computes innovation gain to match curGain
**
** Links to text:
**
** Arguments:
**
** Word16 curGain current average gain to match
** Word16 *PrevExc previous/current excitation (updated)
** Word16 *DataEXc current frame excitation
** Word16 *nRandom random generator status (input/output)
**
** Outputs:
**
** Word16 *PrevExc
** Word16 *DataExc
** Word16 *nRandom
**
** Return value: None
**
*/
void Calc_Exc_Rand(Word16 curGain, Word16 *PrevExc, Word16 *DataExc,
Word16 *nRandom, LINEDEF *Line)
{
int i, i_subfr, iblk;
Word16 temp, temp2;
Word16 j;
Word16 TabPos[2*NbPulsBlk], TabSign[2*NbPulsBlk];
Word16 *ptr_TabPos, *ptr_TabSign;
Word16 *ptr1, *curExc;
Word16 sh1, x1, x2, inter_exc, delta, b0;
Word32 L_acc, L_c, L_temp;
Word16 tmp[SubFrLen/Sgrid];
Word16 offset[SubFrames];
Word16 tempExc[SubFrLenD];
/*
* generate LTP codes
*/
Line->Olp[0] = random_number(21, nRandom) + (Word16)123;
Line->Olp[1] = random_number(21, nRandom) + (Word16)123;
for(i_subfr=0; i_subfr<SubFrames; i_subfr++) { /* in [1, NbFilt] */
Line->Sfs[i_subfr].AcGn = random_number(NbFilt, nRandom) + (Word16)1;
}
Line->Sfs[0].AcLg = 1;
Line->Sfs[1].AcLg = 0;
Line->Sfs[2].AcLg = 1;
Line->Sfs[3].AcLg = 3;
/*
* Random innovation :
* Selection of the grids, signs and pulse positions
*/
/* Signs and Grids */
ptr_TabSign = TabSign;
ptr1 = offset;
for(iblk=0; iblk<SubFrames/2; iblk++) {
temp = random_number((1 << (NbPulsBlk+2)), nRandom);
*ptr1++ = temp & (Word16)0x0001;
temp = shr(temp, 1);
*ptr1++ = add( (Word16) SubFrLen, (Word16) (temp & 0x0001) );
for(i=0; i<NbPulsBlk; i++) {
*ptr_TabSign++= shl(sub((temp & (Word16)0x0002), 1), 14);
temp = shr(temp, 1);
}
}
/* Positions */
ptr_TabPos = TabPos;
for(i_subfr=0; i_subfr<SubFrames; i_subfr++) {
for(i=0; i<(SubFrLen/Sgrid); i++) tmp[i] = (Word16)i;
temp = (SubFrLen/Sgrid);
for(i=0; i<Nb_puls[i_subfr]; i++) {
j = random_number(temp, nRandom);
*ptr_TabPos++ = add(shl(tmp[(int)j],1), offset[i_subfr]);
temp = sub(temp, 1);
tmp[(int)j] = tmp[(int)temp];
}
}
/*
* Compute fixed codebook gains
*/
ptr_TabPos = TabPos;
ptr_TabSign = TabSign;
curExc = DataExc;
i_subfr = 0;
for(iblk=0; iblk<SubFrames/2; iblk++) {
/* decode LTP only */
Decod_Acbk(curExc, &PrevExc[0], Line->Olp[iblk],
Line->Sfs[i_subfr].AcLg, Line->Sfs[i_subfr].AcGn);
Decod_Acbk(&curExc[SubFrLen], &PrevExc[SubFrLen], Line->Olp[iblk],
Line->Sfs[i_subfr+1].AcLg, Line->Sfs[i_subfr+1].AcGn);
temp2 = 0;
for(i=0; i<SubFrLenD; i++) {
temp = abs_s(curExc[i]);
if(temp > temp2) temp2 = temp;
}
if(temp2 == 0) sh1 = 0;
else {
sh1 = sub(4,norm_s(temp2)); /* 4 bits of margin */
if(sh1 < -2) sh1 = -2;
}
L_temp = 0L;
for(i=0; i<SubFrLenD; i++) {
temp = shr(curExc[i], sh1); /* left if sh1 < 0 */
L_temp = L_mac(L_temp, temp, temp);
tempExc[i] = temp;
} /* ener_ltp x 2**(-2sh1+1) */
L_acc = 0L;
for(i=0; i<NbPulsBlk; i++) {
L_acc = L_mac(L_acc, tempExc[(int)ptr_TabPos[i]], ptr_TabSign[i]);
}
inter_exc = extract_h(L_shl(L_acc, 1)); /* inter_exc x 2-sh1 */
/* compute SubFrLenD x curGain**2 x 2**(-2sh1+1) */
/* curGain input = 2**5 curGain */
// L_acc = L_mult(curGain, SubFrLen);
L_MULT(curGain, SubFrLen, L_acc);
L_acc = L_shr(L_acc, 6);
temp = extract_l(L_acc); /* SubFrLen x curGain : avoids overflows */
// L_acc = L_mult(temp, curGain);
L_MULT(temp, curGain, L_acc);
temp = shl(sh1, 1);
temp = add(temp, 4);
L_acc = L_shr(L_acc, temp); /* SubFrLenD x curGain**2 x 2**(-2sh1+1) */
/* c = (ener_ltp - SubFrLenD x curGain**2)/nb_pulses_blk */
/* compute L_c = c >> 2sh1-1 */
L_acc = L_sub(L_temp, L_acc);
/* x 1/nb_pulses_blk */
L_c = L_mls(L_acc, InvNbPulsBlk);
/*
* Solve EQ(X) = X**2 + 2 b0 X + c
*/
/* delta = b0 x b0 - c */
b0 = mult_r(inter_exc, InvNbPulsBlk); /* b0 >> sh1 */
L_acc = L_msu(L_c, b0, b0); /* (c - b0**2) >> 2sh1-1 */
L_acc = L_negate(L_acc); /* delta x 2**(-2sh1+1) */
/* Case delta <= 0 */
if(L_acc <= 0) { /* delta <= 0 */
x1 = negate(b0); /* sh1 */
}
/* Case delta > 0 */
else {
delta = Sqrt_lbc(L_acc); /* >> sh1 */
x1 = sub(delta, b0); /* x1 >> sh1 */
x2 = add(b0, delta); /* (-x2) >> sh1 */
if(abs_s(x2) < abs_s(x1)) {
x1 = negate(x2);
}
}
/* Update DataExc */
sh1 = add(sh1, 1);
temp = shl(x1, sh1);
if(temp > (2*Gexc_Max)) temp = (2*Gexc_Max);
if(temp < -(2*Gexc_Max)) temp = -(2*Gexc_Max);
for(i=0; i<NbPulsBlk; i++) {
j = *ptr_TabPos++;
curExc[(int)j] = add(curExc[(int)j], mult(temp,
(*ptr_TabSign++)) );
}
/* update PrevExc */
ptr1 = PrevExc;
for(i=SubFrLenD; i<PitchMax; i++) *ptr1++ = PrevExc[i];
for(i=0; i<SubFrLenD; i++) *ptr1++ = curExc[i];
curExc += SubFrLenD;
i_subfr += 2;
} /* end of loop on LTP blocks */
return;
}
/*
**
** Function: Qua_SidGain()
**
** Description: Quantization of Sid gain
** Pseudo-log quantizer in 3 segments
** 1st segment : length = 16, resolution = 2
** 2nd segment : length = 16, resolution = 4
** 3rd segment : length = 32, resolution = 8
** quantizes a sum of energies
**
** Links to text:
**
** Arguments:
**
** Word16 *Ener table of the energies
** Word16 *shEner corresponding scaling factors
** Word16 nq if nq >= 1 : quantization of nq energies
** for SID gain calculation in function Cod_Cng()
** if nq = 0 : in function Comp_Info(),
** quantization of saved estimated excitation energy
**
** Outputs: None
**
**
** Return value: index of quantized energy
**
*/
Word16 Qua_SidGain(Word16 *Ener, Word16 *shEner, Word16 nq)
{
Word16 temp, iseg, iseg_p1;
Word16 j, j2, k, exp;
Word32 L_x, L_y;
Word16 sh1;
Word32 L_acc;
int i;
if(nq == 0) {
/* Quantize energy saved for frame erasure case */
/* L_x = 2 x average_ener */
temp = shl(*shEner, 1);
temp = sub(16, temp);
L_acc = L_deposit_l(*Ener);
L_acc = L_shl(L_acc, temp); /* may overflow, and >> if temp < 0 */
L_x = L_mls(L_acc, fact[0]);
}
else {
/*
* Compute weighted average of energies
* Ener[i] = enerR[i] x 2**(shEner[i]-14)
* L_x = k[nq] x SUM(i=0->nq-1) enerR[i]
* with k[nq] = 2 x fact_mul x fact_mul / nq x Frame
*/
sh1 = shEner[0];
for(i=1; i<nq; i++) {
if(shEner[i] < sh1) sh1 = shEner[i];
}
for(i=0, L_x=0L; i<nq; i++) {
temp = sub(shEner[i], sh1);
temp = shr(Ener[i], temp);
temp = mult_r(fact[nq], temp);
L_x = L_add(L_x, L_deposit_l(temp));
}
temp = sub(15, sh1);
L_x = L_shl(L_x, temp);
}
/* Quantize L_x */
if(L_x >= L_bseg[2]) return(63);
/* Compute segment number iseg */
if(L_x >= L_bseg[1]) {
iseg = 2;
exp = 4;
}
else {
exp = 3;
if(L_x >= L_bseg[0]) iseg = 1;
else iseg = 0;
}
iseg_p1 = add(iseg,1);
j = shl(1, exp);
k = shr(j,1);
/* Binary search in segment iseg */
for(i=0; i<exp; i++) {
temp = add(base[iseg], shl(j, iseg_p1));
// L_y = L_mult(temp, temp);
L_MULT(temp, temp, L_y);
if(L_x >= L_y) j = add(j, k);
else j = sub(j, k);
k = shr(k, 1);
}
temp = add(base[iseg], shl(j, iseg_p1));
L_y = L_mult(temp, temp);
L_y = L_sub(L_y, L_x);
if(L_y <= 0L) {
j2 = add(j, 1);
temp = add(base[iseg], shl(j2, iseg_p1));
L_acc = L_mult(temp, temp);
L_acc = L_sub(L_x, L_acc);
if(L_y > L_acc) temp = add(shl(iseg,4), j);
else temp = add(shl(iseg,4), j2);
}
else {
j2 = sub(j, 1);
temp = add(base[iseg], shl(j2, iseg_p1));
L_acc = L_mult(temp, temp);
L_acc = L_sub(L_x, L_acc);
if(L_y < L_acc) temp = add(shl(iseg,4), j);
else temp = add(shl(iseg,4), j2);
}
return(temp);
}
/*
**
** Function: LsptoA()
**
** Description: Converts LSP frequencies to LPC coefficients
** for a subframe. Sum and difference
** polynomials are computed from the LSP
** frequencies (which are the roots of these
** polynomials). The LPC coefficients are then
** computed by adding the sum and difference
** polynomials.
**
** Links to text: Sections 2.7, 3.3
**
** Arguments:
**
** Word16 Lsp[] LSP frequencies (10 words)
**
** Outputs:
**
** Word16 Lsp[] LPC coefficients (10 words)
**
** Return value: None
**
*/
void LsptoA( Word16 *Lsp )
{
int i,j ;
Word32 Acc0,Acc1 ;
Word16 Tmp ;
Word32 P[LpcOrder/2+1] ;
Word32 Q[LpcOrder/2+1] ;
/*
* Compute the cosines of the LSP frequencies by table lookup and
* linear interpolation
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
/*
* Do the table lookup using bits [15:7] of the LSP frequency
*/
j = (int) shr( Lsp[i], (Word16) 7 ) ;
Acc0 = L_deposit_h( CosineTable[j] ) ;
/*
* Do the linear interpolations using bits [6:0] of the LSP
* frequency
*/
Tmp = sub(CosineTable[j+1], CosineTable[j] ) ;
Acc0 = L_mac( Acc0, Tmp, add( shl( (Word16)(Lsp[i] & 0x007f) ,
(Word16)8 ), (Word16) 0x0080 ) ) ;
Acc0 = L_shl( Acc0, (Word16) 1 ) ;
Lsp[i] = negate( round( Acc0 ) ) ;
}
/*
* Compute the sum and difference polynomials with the real roots
* removed. These are computed by polynomial multiplication as
* follows. Let the sum polynomial be P(z). Define the elementary
* polynomials P_i(z) = 1 - 2cos(w_i) z^{-1} + z^{-2}, for 1<=i<=
* 5, where {w_i} are the LSP frequencies corresponding to the sum
* polynomial. Then P(z) = P_1(z)P_2(z)...P_5(z). Similarly
* the difference polynomial Q(z) = Q_1(z)Q_2(z)...Q_5(z).
*/
/*
* Initialize the arrays with the coefficients of the product
* P_1(z)P_2(z) and Q_1(z)Q_2(z). Scale by 1/8.
*/
P[0] = (Word32) 0x10000000L ;
P[1] = L_mult( Lsp[0], (Word16) 0x2000 ) ;
P[1] = L_mac( P[1], Lsp[2], (Word16) 0x2000 ) ;
P[2] = L_mult( Lsp[0], Lsp[2] ) ;
P[2] = L_shr( P[2], (Word16) 1 ) ;
P[2] = L_add( P[2], (Word32) 0x20000000L ) ;
Q[0] = (Word32) 0x10000000L ;
Q[1] = L_mult( Lsp[1], (Word16) 0x2000 ) ;
Q[1] = L_mac( Q[1], Lsp[3], (Word16) 0x2000 ) ;
Q[2] = L_mult( Lsp[1], Lsp[3] ) ;
Q[2] = L_shr( Q[2], (Word16) 1 ) ;
Q[2] = L_add( Q[2], (Word32) 0x20000000L ) ;
/*
* Compute the intermediate polynomials P_1(z)P_2(z)...P_i(z) and
* Q_1(z)Q_2(z)...Q_i(z), for i = 2, 3, 4. Each intermediate
* polynomial is symmetric, so only the coefficients up to i+1 need
* by computed. Scale by 1/2 each iteration for a total of 1/8.
*/
for ( i = 2 ; i < LpcOrder/2 ; i ++ ) {
/* Compute coefficient (i+1) */
Acc0 = P[i] ;
Acc0 = L_mls( Acc0, Lsp[2*i+0] ) ;
// Acc0 = L_add( Acc0, P[i-1] ) ;
L_ADD(Acc0, P[i-1], Acc0);
P[i+1] = Acc0 ;
Acc1 = Q[i] ;
Acc1 = L_mls( Acc1, Lsp[2*i+1] ) ;
// Acc1 = L_add( Acc1, Q[i-1] ) ;
L_ADD(Acc1, Q[i-1], Acc1);
Q[i+1] = Acc1 ;
/* Compute coefficients i, i-1, ..., 2 */
for ( j = i ; j >= 2 ; j -- ) {
Acc0 = P[j-1] ;
Acc0 = L_mls( Acc0, Lsp[2*i+0] ) ;
Acc0 = L_add( Acc0, L_shr(P[j], (Word16) 1 ) ) ;
Acc0 = L_add( Acc0, L_shr(P[j-2], (Word16) 1 ) ) ;
P[j] = Acc0 ;
Acc1 = Q[j-1] ;
Acc1 = L_mls( Acc1, Lsp[2*i+1] ) ;
Acc1 = L_add( Acc1, L_shr(Q[j], (Word16) 1 ) ) ;
Acc1 = L_add( Acc1, L_shr(Q[j-2], (Word16) 1 ) ) ;
Q[j] = Acc1 ;
}
/* Compute coefficients 1, 0 */
P[0] = L_shr( P[0], (Word16) 1 ) ;
Q[0] = L_shr( Q[0], (Word16) 1 ) ;
Acc0 = L_deposit_h( Lsp[2*i+0] ) ;
Acc0 = L_shr( Acc0, (Word16) i ) ;
Acc0 = L_add( Acc0, P[1] ) ;
Acc0 = L_shr( Acc0, (Word16) 1 ) ;
P[1] = Acc0 ;
Acc1 = L_deposit_h( Lsp[2*i+1] ) ;
Acc1 = L_shr( Acc1, (Word16) i ) ;
Acc1 = L_add( Acc1, Q[1] ) ;
Acc1 = L_shr( Acc1, (Word16) 1 ) ;
Q[1] = Acc1 ;
}
/*
* Convert the sum and difference polynomials to LPC coefficients
* The LPC polynomial is the sum of the sum and difference
* polynomials with the real zeros factored in: A(z) = 1/2 {P(z) (1
* + z^{-1}) + Q(z) (1 - z^{-1})}. The LPC coefficients are scaled
* here by 16; the overall scale factor for the LPC coefficients
* returned by this function is therefore 1/4.
*/
for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {
Acc0 = P[i] ;
Acc0 = L_add( Acc0, P[i+1] ) ;
Acc0 = L_sub( Acc0, Q[i] ) ;
Acc0 = L_add( Acc0, Q[i+1] ) ;
Acc0 = L_shl( Acc0, (Word16) 3 ) ;
Lsp[i] = negate( round( Acc0 ) ) ;
Acc1 = P[i] ;
Acc1 = L_add( Acc1, P[i+1] ) ;
Acc1 = L_add( Acc1, Q[i] ) ;
Acc1 = L_sub( Acc1, Q[i+1] ) ;
Acc1 = L_shl( Acc1, (Word16) 3 ) ;
Lsp[LpcOrder-1-i] = negate( round( Acc1 ) ) ;
}
}
void Update_Err(Word16 Olp, Word16 AcLg, Word16 AcGn,ENC_HANDLE *handle)
{
Word16 *ptr_tab;
Word16 i, iz, temp1, temp2;
Word16 Lag;
Word32 Worst1, Worst0, L_temp;
Word16 beta;
Lag = Olp - (Word16)Pstep + AcLg;
/* Select Quantization tables */
i = 0 ;
ptr_tab = tabgain85;
if ( handle->mode == G723_63 ) {
if ( Olp >= (Word16) (SubFrLen-2) ) ptr_tab = tabgain170;
}
else {
ptr_tab = tabgain170;
}
beta = ptr_tab[(int)AcGn]; /* beta = gain * 8192 */
if(Lag <= (SubFrLen/2)) {
Worst0 = L_mls(handle->CodStat.Err[0], beta);
Worst0 = L_shl(Worst0, 2);
Worst0 = L_add(Err0, Worst0);
Worst1 = Worst0;
}
else {
iz = mult(Lag, 1092); /* Lag / 30 */
temp1 = add(iz, 1);
temp2 = sub(shl(temp1, 5), shl(temp1, 1)); /* 30 (iz+1) */
if(temp2 != Lag) {
if(iz == 1) {
Worst0 = L_mls(handle->CodStat.Err[0], beta);
Worst0 = L_shl(Worst0, 2);
Worst0 = L_add(Err0, Worst0);
Worst1 = L_mls(handle->CodStat.Err[1], beta);
Worst1 = L_shl(Worst1, 2);
Worst1 = L_add(Err0, Worst1);
if(Worst0 > Worst1) Worst1 = Worst0;
else Worst0 = Worst1;
}
else {
Worst0 = L_mls(handle->CodStat.Err[iz-2], beta);
Worst0 = L_shl(Worst0, 2);
Worst0 = L_add(Err0, Worst0);
L_temp = L_mls(handle->CodStat.Err[iz-1], beta);
L_temp = L_shl(L_temp, 2);
L_temp = L_add(Err0, L_temp);
if(L_temp > Worst0) Worst0 = L_temp;
Worst1 = L_mls(handle->CodStat.Err[iz], beta);
Worst1 = L_shl(Worst1, 2);
Worst1 = L_add(Err0, Worst1);
if(L_temp > Worst1) Worst1 = L_temp;
}
}
else { /* Lag % SubFrLen = 0 */
Worst0 = L_mls(handle->CodStat.Err[iz-1], beta);
Worst0 = L_shl(Worst0, 2);
Worst0 = L_add(Err0, Worst0);
Worst1 = L_mls(handle->CodStat.Err[iz], beta);
Worst1 = L_shl(Worst1, 2);
Worst1 = L_add(Err0, Worst1);
}
}
for(i=4; i>=2; i--) {
handle->CodStat.Err[i] = handle->CodStat.Err[i-2];
}
handle->CodStat.Err[0] = Worst0;
handle->CodStat.Err[1] = Worst1;
return;
}
Flag Comp_Vad( Word16 *Dpnt)
{
int i,j ;
Word32 Acc0,Acc1 ;
Word16 Tm0, Tm1, Tm2 ;
Word16 Minp ;
Flag VadState = 1 ;
static Word16 ScfTab[11] = {
9170 ,
9170 ,
9170 ,
9170 ,
10289 ,
11544 ,
12953 ,
14533 ,
16306 ,
18296 ,
20529 ,
} ;
if ( !UseVx )
return VadState ;
/* Find Minimum pitch period */
Minp = PitchMax ;
for ( i = 0 ; i < 4 ; i ++ ) {
if ( Minp > VadStat->Polp[i] )
Minp = VadStat->Polp[i] ;
}
/* Check that all are multiplies of the minimum */
Tm2 = 0 ;
for ( i = 0 ; i < 4 ; i ++ ) {
Tm1 = Minp ;
for ( j = 0 ; j < 8 ; j ++ ) {
Tm0 = sub( Tm1, VadStat->Polp[i] ) ;
Tm0 = abs_s( Tm0 ) ;
if ( Tm0 <= 3 )
Tm2 ++ ;
Tm1 = add( Tm1, Minp ) ;
}
}
/* Update adaptation enable counter if not periodic and not sine */
if ( (Tm2 == 4) || (CodStat->SinDet < 0) )
VadStat->Aen += 2 ;
else
VadStat->Aen -- ;
/* Clip it */
if ( VadStat->Aen > 6 )
VadStat->Aen = 6 ;
if ( VadStat->Aen < 0 )
VadStat->Aen = 0 ;
/* Inverse filter the data */
Acc1 = 0L ;
for ( i = SubFrLen ; i < Frame ; i ++ ) {
Acc0 = L_mult( Dpnt[i], 0x2000 ) ;
for ( j = 0 ; j < LpcOrder ; j ++ )
Acc0 = L_msu( Acc0, Dpnt[i-j-1], VadStat->NLpc[j] ) ;
Tm0 = g723_round( Acc0 ) ;
Acc1 = L_mac( Acc1, Tm0, Tm0 ) ;
}
/* Scale the rezidual energy */
Acc1 = L_mls( Acc1, (Word16) 2913 ) ;
/* Clip noise level in any case */
if ( VadStat->Nlev > VadStat->Penr ) {
Acc0 = L_sub( VadStat->Penr, L_shr( VadStat->Penr, 2 ) ) ;
VadStat->Nlev = L_add( Acc0, L_shr( VadStat->Nlev, 2 ) ) ;
}
/* Update the noise level, if adaptation is enabled */
if ( !VadStat->Aen ) {
VadStat->Nlev = L_add( VadStat->Nlev, L_shr( VadStat->Nlev, 5 ) ) ;
}
/* Decay Nlev by small amount */
else {
VadStat->Nlev = L_sub( VadStat->Nlev, L_shr( VadStat->Nlev,11 ) ) ;
}
/* Update previous energy */
VadStat->Penr = Acc1 ;
/* CLip Noise Level */
if ( VadStat->Nlev < 0x00000080L )
VadStat->Nlev = 0x00000080L ;
if ( VadStat->Nlev > 0x0001ffffL )
VadStat->Nlev = 0x0001ffffL ;
/* Compute the treshold */
Acc0 = L_shl( VadStat->Nlev, 13 ) ;
Tm0 = norm_l( Acc0 ) ;
Acc0 = L_shl( Acc0, Tm0 ) ;
Acc0 &= 0x3f000000L ;
Acc0 <<= 1 ;
Tm1 = extract_h( Acc0 ) ;
Acc0 = L_deposit_h( ScfTab[Tm0] ) ;
Acc0 = L_mac( Acc0, Tm1, ScfTab[Tm0-1] ) ;
Acc0 = L_msu( Acc0, Tm1, ScfTab[Tm0] ) ;
Tm1 = extract_h( Acc0 ) ;
Tm0 = extract_l( L_shr( VadStat->Nlev, 2 ) ) ;
Acc0 = L_mult( Tm0, Tm1 ) ;
Acc0 >>= 11 ;
/* Compare with the treshold */
if ( Acc0 > Acc1 )
VadState = 0 ;
/* Do the various counters */
if ( VadState ) {
VadStat->Vcnt ++ ;
VadStat->Hcnt ++ ;
}
else {
VadStat->Vcnt -- ;
if ( VadStat->Vcnt < 0 )
VadStat->Vcnt = 0 ;
}
if ( VadStat->Vcnt >= 2 ) {
VadStat->Hcnt = 6 ;
if ( VadStat->Vcnt >= 3 )
VadStat->Vcnt = 3 ;
}
if ( VadStat->Hcnt ) {
VadState = 1 ;
if ( VadStat->Vcnt == 0 )
VadStat->Hcnt -- ;
}
/* Update Periodicy detector */
VadStat->Polp[0] = VadStat->Polp[2] ;
VadStat->Polp[1] = VadStat->Polp[3] ;
return VadState ;
}
