Commit eb836cef authored by Arthur Tritthart's avatar Arthur Tritthart
Browse files

added draft version of GetF0, incl. division (newton) call and CorrectF0 subroutine

parent 0c0f9d93

lib_dec/TonalComponentDetection_fx.c

100644 → 100755
+104 −58
Original line number Diff line number Diff line
@@ -43,7 +43,11 @@ static void GetF0( Word16 const nSamples, Word16 const nSamplesCore, Word32 cons 
#endif

);

static void findStrongestHarmonics( const Word16 nSamples, const Word32 *powerSpectrum, const Word16 F0, const Word16 nTotalHarmonics, Word16 *pHarmonicIndexes, Word16 *pnHarmonics );

#ifdef FIX_ISSUE_1966

static void CorrectF0( const Word16 *pHarmonicIndexes, const Word16 nHarmonics, Word16 *pF0, Word32 *pF0_32 );

#else

static void CorrectF0( const Word16 *pHarmonicIndexes, const Word16 nHarmonics, Word16 *pF0 );

#endif

#ifdef FIX_ISSUE_1966

static void findCandidates( const Word16 nSamples, const Word32 *MDCTSpectrum, const Word16 MDCTSpectrum_exp, 

                                  Word16 *thresholdModificationNew, Word32 *thresholdModificationNew32, Word16 floorPowerSpectrum );
@@ -172,7 +176,7 @@ void DetectTonalComponents_fx( 
    Word32 sns_int_scf_fx[FDNS_NPTS]; /*Q16*/

    Word16 q_pScaledMdctSpectrum;

#ifdef DEBUG_ISSUE_1966

    printf("Calling DetectTonalComponents *pNumIndexes=%d\n", *pNumIndexes);

    printf("Calling DetectTonalComponents in frame=%d\n", frame);

#endif



    set32_fx( pScaledMdctSpectrum, 0, L_FRAME_MAX );
@@ -549,33 +553,31 @@ static void GetF0( 


        /**pF0 = nSamplesCore/tmpPitchLag;*/

        BASOP_Util_Divide_MantExp( nSamplesCore, 0, tmpPitchLag, -( 1 /*division by 2*/ + 4 /*accommodate accuracy-prevention-leftshift*/ ), pF0, &tmp ); /*pF0 is Q15*/

        move16();

#ifdef FIX_ISSUE_1966

        int tmp_32 = 0;

        double k = nSamplesCore / (tmpPitchLag*0.5f*0.0625f);

        *pF0_32 = double_to_q31_with_exp(k, &tmp_32);

#ifdef DEBUG_ISSUE_1966

        static float err_max=0.f;

        double f1 = *pF0 * pow(2.f, -15+tmp);

        float tmp2= err_max;

        err_max= fmax( err_max, (f1-k)/k );

        if (err_max != tmp2) 

            printf("# frame=%d fx32=0x%08x fx16=0x%04x err=%0.5f,%f,   exp: tmp=%d, tmp_32=%d \n",

                            frame,    *pF0_32,    *pF0,    (f1-k)/k,err_max,     tmp,      tmp_32 );

#endif

        //assert(tmp_32 == tmp);

        if (tmp_32 != tmp) printf("\n\n ERROR L:%d, tmp,tmp_32 = %d,%d\n\n\n", __LINE__, tmp,tmp_32);

#endif

        *pF0 = shr_sat( *pF0, sub( 5, tmp ) ); /*Q10 without scalingfactor*/

#ifdef FIX_ISSUE_1966

        *pF0_32 = L_shr_sat( *pF0_32, sub( 5, tmp_32 ) ); /*Q10 without scalingfactor*/

#endif

        move16();

        *pOrigF0 = *pF0; /*Q10*/

        move16();



#ifdef FIX_ISSUE_1966

        Word16 tmp_32 = 0;

        Word32 F0_32 = BASOP_Util_Divide3232_Scale_newton(L_shl(nSamplesCore, Q16), tmpPitchLag, &tmp_32);

        if (tmp_32 != Q16)

            F0_32 = L_shl(F0_32, tmp_32 - Q16);

        *pF0_32 = F0_32;

        move32();

        *pOrigF0_32 = *pF0_32; /*Q26*/

        move32();

#endif

        move16();

#ifdef DEBUG_ISSUE_1966

        static double err_max = 0.0;

        double f0 = (double) *pF0 / ( 1 << 10 );

        double f0_32 = (double) *pF0_32 / ( 1 << 26 );

        double err = fabs( f0 - f0_32 );

        err_max = fmax( err_max, err );

        printf( "# GetF0 frame=%d *F0_32=0x%08x (%5.10f)  *F0=0x%04x (%5.10f)  err_max=%5.10f  err=%5.10f, tmp_32=%d, tmp=%d  \n",

                  frame, *pF0_32, f0_32, *pF0, f0, err_max, err, tmp_32, tmp);

#endif



        tmp = 2 * LAST_HARMONIC_POS_TO_CHECK;

        if ( LT_16( nSamples, 2 * LAST_HARMONIC_POS_TO_CHECK ) )

        {
@@ -583,32 +585,19 @@ static void GetF0( 
            tmp = nSamples;

        }

#ifdef FIX_ISSUE_1966

        double k2 = ((Word32)tmp<<15) / (*pF0_32*pow(2.,-16+5));

        Word32 nTotalHarmonics_32 = double_to_q31_with_exp(k2, &tmp_32);

#endif

        BASOP_Util_Divide_MantExp( tmp, 15, *pF0, 5, &nTotalHarmonics, &tmp );

#ifdef DEBUG_ISSUE_1966

        if (tmp != tmp_32) printf("tmp=%d  tmp_32=%d\n", tmp,tmp_32 );

        assert( tmp == tmp_32 );

#endif

#ifdef DEBUG_ISSUE_1966__

        printf("# frame=%d k2=%f fx16=%f,  \n",

                        frame, k2,     nTotalHarmonics*pow(2.,-15+tmp ) );

#endif

        BASOP_Util_Divide_MantExp( tmp, 15, extract_h( *pF0_32 ), 5, &nTotalHarmonics, &tmp );

        nTotalHarmonics = shl( nTotalHarmonics, sub( tmp, 15 ) );

#ifdef FIX_ISSUE_1966

        nTotalHarmonics_32 = L_shl( nTotalHarmonics_32, L_sub( tmp_32, 31 ) );

#ifdef DEBUG_ISSUE_1966___

        if (nTotalHarmonics != nTotalHarmonics_32)

            printf("# nTotalHarmonics=%d nTotalHarmonics_32=%d\n",

                                      nTotalHarmonics,      nTotalHarmonics_32 );

#endif

#endif



        /* Get in rgiStrongHarmonics all i for which i*F0 are the strongest harmonics */

        findStrongestHarmonics( nSamples, powerSpectrum, extract_h( *pF0_32 ), nTotalHarmonics, rgiStrongHarmonics, &nStrongHarmonics );

        CorrectF0( rgiStrongHarmonics, nStrongHarmonics, pF0, pF0_32 );

#else

        BASOP_Util_Divide_MantExp( tmp, 15, *pF0, 5, &nTotalHarmonics, &tmp );

        nTotalHarmonics = shl( nTotalHarmonics, sub( tmp, 15 ) );

        /* Get in rgiStrongHarmonics all i for which i*F0 are the strongest harmonics */

        findStrongestHarmonics( nSamples, powerSpectrum, *pF0, nTotalHarmonics, rgiStrongHarmonics, &nStrongHarmonics );



        CorrectF0( rgiStrongHarmonics, nStrongHarmonics, pF0 );

#endif

    }

    ELSE

    {
@@ -616,6 +605,12 @@ static void GetF0( 
        move16();

        *pF0 = 0;

        *pOrigF0 = 0;

#ifdef FIX_ISSUE_1966

        move32();

        move32();

        *pF0_32 = 0;

        *pOrigF0_32 = 0;

#endif

    }



    return;
@@ -711,10 +706,18 @@ static void findStrongestHarmonics( 
static void CorrectF0(

    const Word16 /*short*/ *pHarmonicIndexes, /*I    - Q0  */

    const Word16 /*short*/ nHarmonics,        /*I    - Q0  */

#ifdef FIX_ISSUE_1966

    Word16 /*short*/ *pF0,                    /*I/O  - Q10 range: {0}, [4..18) */

    Word32 /*long*/  *pF0_32 )                /*I/O  - Q10+16 range: {0}, [4..18) */

#else

    Word16 /*short*/ *pF0 ) /*I/O  - Q10 range: {0}, [4..18) */

#endif

{

    Word16 /*short*/ i;

    Word16 /*short*/ F0;

#ifdef FIX_ISSUE_1966

    Word32 F0_32;

#endif

    Word16 /*short*/ diff[MAX_PEAKS_FROM_PITCH - 1], sortedDiff[MAX_PEAKS_FROM_PITCH - 1]; /*Q0*/

    Word16 /*short*/ iMostCommonDiff, nMostCommonDiff, nSameDiff, iMult;                   /*Q0*/
@@ -729,10 +732,26 @@ static void CorrectF0( 
    }



    F0 = *pF0; /*Q10*/

#ifdef FIX_ISSUE_1966

    F0_32 = *pF0_32;

#endif



#ifdef DEBUG_ISSUE_1966

    /* Condition should be identical for 16 or 32 version */

    if ( ( F0 == 0 ) && ( F0_32 != 0 ) )

        printf("CorrectF0: F0 (0x%04X) != F0_32 (0x%08X)\n", F0, F0_32);

#endif



    test();

#ifdef FIX_ISSUE_1966

    IF( F0_32 > 0 && nHarmonics != 0 )

#else

    IF( F0 > 0 && nHarmonics != 0 )

#endif

    {

#ifdef DEBUG_ISSUE_1966

        printf( "CorrectF0: - start - F0 (0x%04X), F0_32 (0x%08X)\n", F0, F0_32 );

#endif

        tmp = sub( nHarmonics, 1 );

        FOR( i = 0; i < tmp; i++ )

        {
@@ -836,11 +855,24 @@ static void CorrectF0( 
            {

                /* Use iMostCommonDiff, because the lowest pHarmonicIndexes[i] (which is equal to iMult) may not correspond to the new F0, but to it's multiple */

                F0 = round_fx_sat( L_shl_sat( L_mult( iMostCommonDiff /*Q0*/, F0 /*Q10*/ ), 15 ) ); /*Q10*/

#ifdef FIX_ISSUE_1966

                F0_32 = L_shl_sat( Mpy_32_16_1( F0_32 /*Q10+16*/, iMostCommonDiff /*Q0*/ ), 15 ); /*Q10+16*/

#endif

#ifdef DEBUG_ISSUE_1966

                printf("CorrectF0: A F0_32=0x%08X  F0=0x%04X frame=%d\n", F0_32,F0, frame );

#endif

            }

            ELSE

            {

#ifdef FIX_ISSUE_1966

                F0_32 = 0;

                move32();

#endif

                F0 = 0;

                move16();

#ifdef DEBUG_ISSUE_1966

                printf( "CorrectF0: B F0_32=0x%08X  F0=0x%04X frame=%d\n", F0_32, F0, frame );

#endif

            }

        }

        /* Otherwise if there are at least 3 distances between peaks with length 1 and if the 1st harmonic is in pHarmonicIndexes then keep the original F0 */
@@ -851,13 +883,27 @@ static void CorrectF0( 
            if ( ( GT_16( iMostCommonDiff, 1 ) ) || ( LT_16( nMostCommonDiff, 3 ) ) )

            {

                /* Not enough peaks at the same distance => don't use the pitch. */

#ifdef FIX_ISSUE_1966

                F0_32 = 0;

                move32();

#endif

                F0 = 0;

                move16();

#ifdef DEBUG_ISSUE_1966

                printf( "CorrectF0: C F0_32=0x%08X  F0=0x%04X frame=%d\n", F0_32, F0, frame );

#endif

            }

        }

#ifdef FIX_ISSUE_1966

        *pF0_32 = F0_32;

        move32();

#endif

        *pF0 = F0;

        move16();

    }

#ifdef DEBUG_ISSUE_1966

    printf( "CorrectF0: D F0_32=0x%08X  F0=0x%04X frame=%d\n", F0_32, F0, frame );

#endif



    return;

}
@@ -937,15 +983,15 @@ static void modifyThresholds( 
            FOR( i = 1; i <= nHarmonics; i++ )

            {

                modifyThreshold( i, origF0,

#ifdef DEBUG_ISSUE_1966

#ifdef FIX_ISSUE_1966

                                 origF0_32,

#endif

                                 717 /*0.7f Q10*/ /*0.7f in Q10*/,

#ifdef DEBUG_ISSUE_1966

                                 717 * 0x10000,

                                 717 /*0.7f in Q10*/,

#ifdef FIX_ISSUE_1966

                                 46976205 /*0.7f in Q10+16*/ ,

#endif

                                 thresholdModification

#ifdef DEBUG_ISSUE_1966

#ifdef FIX_ISSUE_1966

                                 ,

                                 thresholdModification_32

#endif
@@ -963,15 +1009,15 @@ static void modifyThresholds( 
            FOR( i = tmp; i > 0; i-- )

            {

                modifyThreshold( i, origF0,

#ifdef DEBUG_ISSUE_1966

#ifdef FIX_ISSUE_1966

                                 origF0_32,

#endif

                                 358 /*0.35f Q10*/,

#ifdef DEBUG_ISSUE_1966

                                 358 * 0x10000,

                                 358 /*0.35f in Q10*/,

#ifdef FIX_ISSUE_1966

                                 23488102 /*0.35 in Q10+16*/,

#endif

                                 thresholdModification

#ifdef DEBUG_ISSUE_1966

#ifdef FIX_ISSUE_1966

                                 ,

                                 thresholdModification_32

#endif
@@ -980,15 +1026,15 @@ static void modifyThresholds( 
            FOR( i = 1; i <= nHarmonics; i++ )

            {

                modifyThreshold( i, F0,

#ifdef DEBUG_ISSUE_1966

#ifdef FIX_ISSUE_1966

                                 origF0_32,

#endif

                                 358 /*0.35f Q10*/,

#ifdef DEBUG_ISSUE_1966

                                 358 * 0x10000,

#ifdef FIX_ISSUE_1966

                                 23488102 /*0.35 in Q10+16*/,

#endif

                                 thresholdModification

#ifdef DEBUG_ISSUE_1966

#ifdef FIX_ISSUE_1966

                                 ,

                                 thresholdModification_32

#endif