landzo

【开源】线性CCD自适应性算法攻略

    /**
     * @brief Clips Q63 to Q15 values.
     */
    static __INLINE q15_t clip_q63_to_q15(
        q63_t x)
    {
        return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
               ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
    }

    /**
     * @brief Clips Q31 to Q7 values.
     */
    static __INLINE q7_t clip_q31_to_q7(
        q31_t x)
    {
        return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
               ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
    }

    /**
     * @brief Clips Q31 to Q15 values.
     */
    static __INLINE q15_t clip_q31_to_q15(
        q31_t x)
    {
        return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
               ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
    }

    /**
     * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
     */

    static __INLINE q63_t mult32x64(
        q63_t x,
        q31_t y)
    {
        return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
                (((q63_t) (x >> 32) * y)));
    }


#if defined (ARM_MATH_CM0) && defined ( __CC_ARM   )
#define __CLZ __clz
#endif

#if defined (ARM_MATH_CM0) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )

    static __INLINE  uint32_t __CLZ(q31_t data);


    static __INLINE uint32_t __CLZ(q31_t data)
    {
        uint32_t count = 0;
        uint32_t mask = 0x80000000;

        while((data & mask) ==  0)
        {
            count += 1u;
            mask = mask >> 1u;
        }

        return(count);

    }

#endif

    /**
     * @brief Function to Calculates 1/in(reciprocal) value of Q31 Data type.
     */

    static __INLINE uint32_t arm_recip_q31(
        q31_t in,
        q31_t *dst,
        q31_t *pRecipTable)
    {

        uint32_t out, tempVal;
        uint32_t index, i;
        uint32_t signBits;

        if(in > 0)
        {
            signBits = __CLZ(in) - 1;
        }
        else
        {
            signBits = __CLZ(-in) - 1;
        }

        /* Convert input sample to 1.31 format */
        in = in << signBits;

        /* calculation of index for initial approximated Val */
        index = (uint32_t) (in >> 24u);
        index = (index & INDEX_MASK);

        /* 1.31 with exp 1 */
        out = pRecipTable[index];

        /* calculation of reciprocal value */
        /* running approximation for two iterations */
        for (i = 0u; i < 2u; i++)
        {
            tempVal = (q31_t) (((q63_t) in * out) >> 31u);
            tempVal = 0x7FFFFFFF - tempVal;
            /*      1.31 with exp 1 */
            //out = (q31_t) (((q63_t) out * tempVal) >> 30u);
            out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
        }

        /* write output */
        *dst = out;

        /* return num of signbits of out = 1/in value */
        return (signBits + 1u);

    }

    /**
     * @brief Function to Calculates 1/in(reciprocal) value of Q15 Data type.
     */
    static __INLINE uint32_t arm_recip_q15(
        q15_t in,
        q15_t *dst,
        q15_t *pRecipTable)
    {

        uint32_t out = 0, tempVal = 0;
        uint32_t index = 0, i = 0;
        uint32_t signBits = 0;

        if(in > 0)
        {
            signBits = __CLZ(in) - 17;
        }
        else
        {
            signBits = __CLZ(-in) - 17;
        }

        /* Convert input sample to 1.15 format */
        in = in << signBits;

        /* calculation of index for initial approximated Val */
        index = in >> 8;
        index = (index & INDEX_MASK);

        /*      1.15 with exp 1  */
        out = pRecipTable[index];

        /* calculation of reciprocal value */
        /* running approximation for two iterations */
        for (i = 0; i < 2; i++)
        {
            tempVal = (q15_t) (((q31_t) in * out) >> 15);
            tempVal = 0x7FFF - tempVal;
            /*      1.15 with exp 1 */
            out = (q15_t) (((q31_t) out * tempVal) >> 14);
        }

        /* write output */
        *dst = out;

        /* return num of signbits of out = 1/in value */
        return (signBits + 1);

    }


    /*
     * @brief C custom defined intrinisic function for only M0 processors
     */
#if defined(ARM_MATH_CM0)

    static __INLINE q31_t __SSAT(
        q31_t x,
        uint32_t y)
    {
        int32_t posMax, negMin;
        uint32_t i;

        posMax = 1;
        for (i = 0; i < (y - 1); i++)
        {
            posMax = posMax * 2;
        }

        if(x > 0)
        {
            posMax = (posMax - 1);

            if(x > posMax)
            {
                x = posMax;
            }
        }
        else
        {
            negMin = -posMax;

            if(x < negMin)
            {
                x = negMin;
            }
        }
        return (x);


    }

#endif /* end of ARM_MATH_CM0 */



    /*
     * @brief C custom defined intrinsic function for M3 and M0 processors
     */
#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0)

    /*
     * @brief C custom defined QADD8 for M3 and M0 processors
     */
    static __INLINE q31_t __QADD8(
        q31_t x,
        q31_t y)
    {

        q31_t sum;
        q7_t r, s, t, u;

        r = (char) x;
        s = (char) y;

        r = __SSAT((q31_t) (r + s), 8);
        s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
        t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
        u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);

        sum = (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
              (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);

        return sum;

    }

    /*
     * @brief C custom defined QSUB8 for M3 and M0 processors
     */
    static __INLINE q31_t __QSUB8(
        q31_t x,
        q31_t y)
    {

        q31_t sum;
        q31_t r, s, t, u;

        r = (char) x;
        s = (char) y;

        r = __SSAT((r - s), 8);
        s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
        t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
        u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;

        sum =
            (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r & 0x000000FF);

        return sum;
    }

    /*
     * @brief C custom defined QADD16 for M3 and M0 processors
     */

    /*
     * @brief C custom defined QADD16 for M3 and M0 processors
     */
    static __INLINE q31_t __QADD16(
        q31_t x,
        q31_t y)
    {

        q31_t sum;
        q31_t r, s;

        r = (short) x;
        s = (short) y;

        r = __SSAT(r + s, 16);
        s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;

        sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

        return sum;

    }

    /*
     * @brief C custom defined SHADD16 for M3 and M0 processors
     */
    static __INLINE q31_t __SHADD16(
        q31_t x,
        q31_t y)
    {

        q31_t sum;
        q31_t r, s;

        r = (short) x;
        s = (short) y;

        r = ((r >> 1) + (s >> 1));
        s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;

        sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

        return sum;

    }

    /*
     * @brief C custom defined QSUB16 for M3 and M0 processors
     */
    static __INLINE q31_t __QSUB16(
        q31_t x,
        q31_t y)
    {

        q31_t sum;
        q31_t r, s;

        r = (short) x;
        s = (short) y;

        r = __SSAT(r - s, 16);
        s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;

        sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

        return sum;
    }

    /*
     * @brief C custom defined SHSUB16 for M3 and M0 processors
     */
    static __INLINE q31_t __SHSUB16(
        q31_t x,
        q31_t y)
    {

        q31_t diff;
        q31_t r, s;

        r = (short) x;
        s = (short) y;

        r = ((r >> 1) - (s >> 1));
        s = (((x >> 17) - (y >> 17)) << 16);

        diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);

        return diff;
    }

    /*
     * @brief C custom defined QASX for M3 and M0 processors
     */
    static __INLINE q31_t __QASX(
        q31_t x,
        q31_t y)
    {

        q31_t sum = 0;

        sum = ((sum + clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
              clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));

        return sum;
    }

    /*
     * @brief C custom defined SHASX for M3 and M0 processors
     */
    static __INLINE q31_t __SHASX(
        q31_t x,
        q31_t y)