math.c

TDK 6521 SOC 芯片 DEMO程序

void shift_right8_2 (uint8x_t *x)       // Shift right 8-byte number right 2-bits.
{
    uint8_t carry_a, carry_b;

    carry_a = *x << 6;      *x =           (*x >> 2);  x++;
    carry_b = *x << 6;      *x = carry_a | (*x >> 2);  x++;
    carry_a = *x << 6;      *x = carry_b | (*x >> 2);  x++;
    carry_b = *x << 6;      *x = carry_a | (*x >> 2);  x++;
    carry_a = *x << 6;      *x = carry_b | (*x >> 2);  x++;
    carry_b = *x << 6;      *x = carry_a | (*x >> 2);  x++;
    carry_a = *x << 6;      *x = carry_b | (*x >> 2);  x++;
    carry_b = *x << 6;      *x = carry_a | (*x >> 2);
}     
#else
void shift_right8_2 (uint8x_t *x)       // Shift right 8-byte number right 2-bits.
{
    uint8_t n = 8;
    uint8_t carry_a = 0;
    uint8_t carry_b;

    do
    {
        carry_b = *x << 6;
        *x = carry_a | (*x >> 2);
        carry_a = carry_b;
        x++;
    } while (--n);
}
#endif // MATH_FAST.
#endif // VA_ELEMENT/VA_SUMS/RMS_VALUES/CALIBRATION.
#endif

#if EXTRAS
//===========================================================================//
/*
    square = TWOto30;                            // = 2^30.

    square_root  = TWOto15;                      // = 1000 0000 0000 0000.
    square += TWOto28 + square_root * TWOto15;   // = 1000 0000 0000 0000 * 2^15.
                                                    = 1000 0000 0000 0000 0000 * 2^11.
                                                    
    square_root += TWOto14;                      // = 1100 0000 0000 0000.
    square += TWOto26 + square_root * TWOto14;   // = 1100 0000 0000 0000 * 2^14.
                                                      0110 0000 0000 0000 0000 * 2^11.

    square_root += TWOto13;                      // = 1110 0000 0000 0000.
    square += TWOto24 + square_root * TWOto13;   // = 1110 0000 0000 0000 * 2^13.
                                                      0011 1000 0000 0000 0000 * 2^11. 

    square_root += TWOto12;                      // = 1111 0000 0000 0000.
    square += TWOto22 + square_root * TWOto12;   // = 1111 0000 0000 0000 * 2^12.
                                                      0001 1110 0000 0000 0000 * 2^11.
    .............................................................................
*/

#define TWOto30 0x40000000

// Returns the 16-bit square root of the 32-bit argument 'x'.
uint16_t sqrt4_2 (int32_t x) 
{
    uint32x_t *a;
    uint8_16_32_t square_root;
    uint32_t square, square_tmp;
    uint32_t TWOto2M;
    uint8_t M1;                         // M + 1.
    
    a = (uint32x_t *) x;

    if (*a >= TWOto30)
    {
       square = TWOto30;
       square_root.l = TWOto30;         // = square_root * 2^15.
    }
    else
    {
       square = 0;
       square_root.l = 0;               // = square_root * 2^15.
    }

    TWOto2M = TWOto30 >> 2;             // = 2^M * 2^M.

    M1 = 15;
    do
    {
       square_tmp = TWOto2M + square_root.l + square;
       square_root.l >>= 1;

       if (*a >= square_tmp)
       {
          square = square_tmp;
          square_root.l |= TWOto2M;     // = square_root * 2^M.
       }

       TWOto2M >>= 2;

    } while (--M1);

    square_root.l >>= 1;
    return (* (uint16_t *) &square_root.c[ LO_HI ]);
}
#endif // EXTRAS.

#if AUTOCAL && \
    (EQUATION != _1ELEMENT_3WIRE \
     && EQUATION != _2ELEMENT_4WIRE_DELTA \
     && EQUATION != _2ELEMENT_4WIRE_WYE)
// Convert a 64 bit sum to floating point.  This can underflow 
// by as many as 40 bits, and should only be used for ratiometric
// purposes such as calibration.  In particular, do not try to
// manipulate a billing or accumulated demand register in floating point!
float s2f (uint8x_t *s)
{
    float f;
    int32_t xdata l[2];  // high and low 32-bit words of s

    // take the absolute value of the 64 bit number
    abs_x ((uint8x_t *)&l, s, 8);

    // Add in the 32-bit parts of the 64 bit number.
    // Warning:The target mantissa is only 24 bits.
    f  = ((float) l[ HI ]) * 4294967296.;		// aka 0x1 0000 0000.
    f += ((float) l[ LO ]);

    // find and set the sign of the number
    if (*s > 0x7F)
        f = -f;

    return f;
}
#endif

// standard C99 library function not provided by Keil
// converts a float to the nearest long integer.  Specifically
// handles negative and positive values.
// Keil's float->long conversion has some defects at boundaries.
// e.g. INT32_MIN  (0x8000000) doesn't convert correctly to float.
long lroundf (float f)
{
    if (f < 0.0)
    {
        if (f <= ((float)(-INT32_MAX)))
        {
            return -INT32_MAX;
        }
        else
        {
            return (long)(f - 0.5);
        }
    }
    else
    {
        if (f >= ((float)(INT32_MAX)))
        {
            return INT32_MAX;
        }
        else
        {
            return (long)(f + 0.5);
        }
    }
}
/***************************************************************************
 * History
 * $Log: math.c,v $
 * Revision 1.40  2006/10/13 00:52:35  tvander
 * Removed compile options for 6530, 6515;
 * renamed 6511 and 6513 to trace11 and trace13;
 * Binary verified unchanged from previous version.
 *
 * Revision 1.39  2006/09/10 00:30:30  Michael T. Fischer
 * First version to support DGM0915 LCD.
 *
 * Revision 1.38  2006/09/09 01:15:28  gmikef
 * *** empty log message ***
 *
 * Revision 1.37  2006/09/06 02:27:05  tvander
 * Clean build
 *
 * Revision 1.36  2006/08/30 02:09:53  gmikef
 * *** empty log message ***
 *
 * Revision 1.35  2006/07/07 01:08:04  tvander
 * Fixed misc. compilation warnings by changing the compilation flags for labs()
 *
 * Revision 1.34  2006/06/29 00:58:44  tvander
 * Added NOAREGs to reentrant code.
 *
 * Revision 1.33  2006/06/06 05:15:57  tvander
 * clean build
 *
 * Revision 1.32  2006/05/18 23:18:56  tvander
 * 16K and 32K
 * First cut at new requirements.
 * 32K 6521 is grossly tested.
 * All others have a clean compile with C51 8.02
 *
 * Revision 1.31  2006/04/14 20:21:25  tvander
 * Integrated with phased calibration
 *
 * Revision 1.30  2006/04/12 00:30:40  tvander
 * Added code for phased calibration, 6513 compiled with equations 3 and 4.
 *
 * Revision 1.29  2006/04/06 21:07:47  tvander
 * Build issue
 *
 * Revision 1.28  2006/03/17 00:45:16  tvander
 * IMAX2 needs a 64-bit product multiplication in its current revision
 *
 * Revision 1.27  2006/03/06 03:42:29  Michael T. Fischer
 * More 6530 prep.
 *
 * Revision 1.26  2006/02/10 00:53:50  tvander
 * Added routine to convert 64 bit numbers to floating point.
 *
 * Revision 1.25  2006/02/08 22:54:15  gmikef
 * *** empty log message ***
 *
 * Revision 1.24  2006/01/25 01:03:37  tvander
 * The gain adjustments were ported to floating point, and some fixed point routines became redundant.
 *
 * Revision 1.23  2006/01/16 20:11:32  tvander
 * Clean Keil build, all versions
 *
 * Revision 1.22  2006/01/10 04:12:40  gmikef
 * Added PDATA support for CE Outputs.
 *
 * Revision 1.21  2006/01/04 04:47:56  gmikef
 * Switched RMS and VA calculations to use floating point. (and Calibration).
 *
 * Revision 1.19  2005/10/31 21:58:39  tvander
 * shift8_1() and shift8_2() of the compact math would overwrite a large block of ram because their loop counters were not initialized.
 *
 * Revision 1.18  2005/10/31 17:38:05  tvander
 * Includes improved EEPROM code with uwire.
 * Clean build, all build trees (Thank-you, Mike!)
 *
 * Revision 1.17  2005/09/24 03:15:28  gmikef
 * Fixed "divide_1" function to give proper residue.
 *
 * Revision 1.16  2005/09/22 23:45:27  tvander
 * Clean build all models and unit tests, updated copyright to be fore Teridian
 *
 * Revision 1.15  2005/08/23 02:11:58  gmikef
 * *** empty log message ***
 *
 * Revision 1.14  2005/08/18 20:44:45  tvander
 * Added temperature measurement to GUI-available fields.
 * FIxed memory-space problem in add8_8
 * Added temp_x, temp_nom, ppmc and ppmc2 to register def. file.,
 * moved many other registers, whcih where in the way.
 *
 * Revision 1.13  2005/08/18 17:38:54  gmikef
 * *** empty log message ***
 *
 * Revision 1.12  2005/08/12 06:03:25  gmikef
 * Added MPU temperature compensation for GAIN_ADJ.
 * Added changes to support new CE 6521 code.
 *
 * Revision 1.11  2005/08/10 02:05:52  gmikef
 * *** empty log message ***
 *
 * Revision 1.10  2005/06/25 02:04:45  tvander
 * Integrated pulse counting
 *
 * Revision 1.9  2005/06/17 22:54:46  tvander
 * Separated imports and exports.
 * Some imports and exports were not being updated.
 *
 * Revision 1.8  2005/05/13 00:34:47  tvander
 * 6511/32k works
 * Integrated and debugged self-calibration.
 * The build has one unused segment, and no other errors or warnings.
 * default LCD and pulse displays appear OK.
 * EEPROM, software timers and hardware timers are all integrated.
 *
 * Revision 1.7  2005/05/04 01:00:36  gmikef
 * *** empty log message ***
 *
 * Revision 1.11  2005/05/03 22:29:40  gmikef
 * *** empty log message ***
 *
 * Revision 1.10  2005/05/03 02:18:55  gmikef
 * *** empty log message ***
 *
 * Revision 1.9  2005/05/02 18:03:19  gmikef
 * *** empty log message ***
 *
 * Revision 1.6  2005/04/30 02:19:05  gmikef
 * *** empty log message ***
 *
 * Revision 1.5  2005/04/28 19:12:27  tvander
 * Comments only!  Restored history comments.
 *
 * Revision 1.4  2005/04/27 23:47:23  gmikef
 * Some MATH rountines now use 'idata'.
 * Added MATH_FAST flag to 'options.h".
 * Changed "6521B.Uv2" to max optimization.
 *
 * Revision 1.3  2005/04/21 02:07:32  gmikef
 * *** empty log message ***
 *
 * Revision 1.2  2005/04/20 00:10:41  tvander
 * Revised the comments.  Hopefully they're more readable.
 *
 * Revision 1.1  2005/04/12 21:53:35  tvander
 * Library and math, compiles ok, added some comments (needs more)
 *
 * Copyright (C) 2005 Teridian Semiconductor Corp. All Rights Reserved.    *
 * this program is fully protected by the United States copyright          *
 * laws and is the property of Teridian Semiconductor Corporation.         *
 ***************************************************************************/