Inverse SQRT for fixed-point values

0

I am developing a code for a real-time DSP and I need the experts' feedback on part of the code that uses the method mentioned in "Answer 3 to the post Inverse sqrt for fixed point" to implement 1/sqrt(uint).

The code is being developed for a TMS320C6455 DSP. Thus, the c64x+ intrinsics are used. The comments explains each intrinsic.

I have two arrays (A(I+jQ) and B(I+jQ)) each of which with more than 10 elements of 32-bit imaginary numbers with Q15-Q15 format in high-low 16 bits, repectively. I want to calculate sum(AxB*)/ABS[sum(AxB*)] for elements 1 to 10 (element 0 is not used).

The code multiplies the conjugate of B(1 to 10) by A(1 to 10) sample by sample and sums them to get two 32 bit I and Q values. Then the 16MSB of I and Q are used to calculate I^2+Q^2. Finally using the method mentioned in "Answer 3 above" it calculates a CRot parameter as IQ/abs(IQ). This is performed by computing the 1/SQRT(I^2+Q^2) using the Fxisqrt() function, then multiplying the result by IQ. Here is my code and I would like to get the experts feedback on it. The main concern I have about this code is preserving the accuracy; I am not sure if using the 16MSB at the end of the each multiplication of Q15*Q15 is the right choice! I would appreciate it if the experts in fixed-point programming could help me to get this right.

void ProcessCRot (const unsigned int *restrict A, const unsigned int *restrict B, 
                  unsigned int *restrict CRot)
/* -----------------------------------------------------------------------
    Tricks: The following c64x+ intrinsics are used:
    _mpyh() and _mpy() conduct hi-hi and lo-lo 16-bit multiplications, respectively;
    _mpylh() and _mpyhl() conduct lo-hi and hi-lo 16-bit multiplications, 
    respectively;
    _packh2() packs 16MSB of (a and b) to (hi and lo) 16-bits of output, respectively;
    _mpyhsu(int a, uint b) calculates 16MSB of int a times 16MSB of uint b;
    Timing:
        Takes 286 ns for CPU clock 1 GHz.
   ----------------------------------------------------------------------- */
{
    unsigned int v, m, CRotI, CRotQ, InvMag, SqrMag;
    int I = 0;
    int Q = 0;
    int i;

    /* --- Multiply A(i) by conjugate of B(i) and sum the result into IQ */
    for (i= 0; i < 10; i++) {
        /* ----- Read the complex sample ----- */
        v   =  _amem4_const (++A);          /* start from sample 1 not 0 */
        m   =  _amem4_const (++B);          /* start from sample 1 not 0 */
        I   += _mpyh(v,m) + _mpy(v,m);      /* I1.I2 + Q1.Q2 */
        Q   += _mpylh(v,m) - _mpyhl(v,m);   /* I2.Q1 - I1.Q2 */
    }
    SqrMag       = _mpyh(I,I) + _mpyh(Q,Q); /* I^2 + Q^2 */
    Fxisqrt (SqrMag,&InvMag);
    CRotI        = _mpyhsu (I,InvMag);      /* (16MSB int I)*(16MSB uint InvMag) */
    CRotQ        = _mpyhsu (Q,InvMag);      /* (16MSB int Q)*(16MSB uint InvMag) */

    /* pack 16MSB of CRotI and CRotQ into 16-hi & 16-lo of CRot, respectively */
    _amem4(CRot) = _packh2 (CRotI,CRotQ);
}

void Fxisqrt (unsigned int a, unsigned int *restrict InvSqrt)
/* -------------------------------------------------------------------------
    The function Fxisqrt(a,InvSqrt) below implements 1/sqrt(a).

    The code uses a helper look-up table gLUT[]. Also intrinsic 
    _hill(_mpy32u()) is used that returns the 32 MSBs of a full 64-bit 
    product of two unsigned 32-bit integers.
   ------------------------------------------------------------------------- */
{
    unsigned int s, t, scal, r;

    unsigned int gLUT [96] = {
    0xfa0bdefa, 0xee6af6ee, 0xe5effae5, 0xdaf27ad9,
    0xd2eff6d0, 0xc890aec4, 0xc10366bb, 0xb9a71ab2,
    0xb4da2eac, 0xadce7ea3, 0xa6f2b29a, 0xa279a694,
    0x9beb568b, 0x97a5c685, 0x9163027c, 0x8d4fd276,
    0x89501e70, 0x8563da6a, 0x818ac664, 0x7dc4fe5e,
    0x7a122258, 0x7671be52, 0x72e44a4c, 0x6f68fa46,
    0x6db22a43, 0x6a52623d, 0x67041a37, 0x65639634,
    0x622ffe2e, 0x609cba2b, 0x5d837e25, 0x5bfcfe22,
    0x58fd461c, 0x57838619, 0x560e1216, 0x53300a10,
    0x51c72e0d, 0x50621a0a, 0x4da48204, 0x4c4c2e01,
    0x4af789fe, 0x49a689fb, 0x485a11f8, 0x4710f9f5,
    0x45cc2df2, 0x448b4def, 0x421505e9, 0x40df5de6,
    0x3fadc5e3, 0x3e7fe1e0, 0x3d55c9dd, 0x3d55d9dd,
    0x3c2f41da, 0x39edd9d4, 0x39edc1d4, 0x38d281d1,
    0x37bae1ce, 0x36a6c1cb, 0x3595d5c8, 0x3488f1c5,
    0x3488fdc5, 0x337fbdc2, 0x3279ddbf, 0x317749bc,
    0x307831b9, 0x307879b9, 0x2f7d01b6, 0x2e84ddb3,
    0x2d9005b0, 0x2d9015b0, 0x2c9ec1ad, 0x2bb0a1aa,
    0x2bb0f5aa, 0x2ac615a7, 0x29ded1a4, 0x29dec9a4,
    0x28fabda1, 0x2819e99e, 0x2819ed9e, 0x273c3d9b,
    0x273c359b, 0x2661dd98, 0x258ad195, 0x258af195,
    0x24b71192, 0x24b6b192, 0x23e6058f, 0x2318118c,
    0x2318718c, 0x224da189, 0x224dd989, 0x21860d86,
    0x21862586, 0x20c19183, 0x20c1b183, 0x20001580 };

    if ( a == 0 ) {
        _amem4 (InvSqrt) = (~a); /* handle special case of zero input */
    } else {
        /* normalize argument */
        r = 32;
        if (a >= 65536) { a >>= 16; r -= 16; }
        if (a >=   256) { a >>=  8; r -=  8; }
        if (a >=    16) { a >>=  4; r -=  4; }
        if (a >=     4) { a >>=  2; r -=  2; }
        r -= (a - (a & (a >> 1)));
        scal = (r & 0xfffffffe);
        a = (a << scal);
        t = gLUT [(a >> 25) - 32];           /* initial approximation */
        r = (t << 22) - _hill(_mpy32u(t,a)); /* first NR iteration */
        s = _hill(_mpy32u(r,a));             /* second NR iteration */
        s = 0x30000000 - _hill(_mpy32u(r,s));/* return 32-MSB of a 64-bit value */
        r = _hill(_mpy32u(r,s));

        /* denormalize and round result to Q0.31 */
        _amem4 (InvSqrt) = (unsigned int) (((r >> (18 - (scal >> 1)) ) + 1) >> 1);
    }
}
fixed-point
asked on Stack Overflow Feb 24, 2020 by masoud

0 Answers

Nobody has answered this question yet.


User contributions licensed under CC BY-SA 3.0