math.c

TDK 6521 SOC 芯片 DEMO程序

    if (v > 1)
    {                                   // Proper divisor.
       t.c[ HI ] = *u;                  // Get current MSB of dividend.

       if (*u >= v)
       {                                // MSB of quotient not zero (0).
          *u /= v;                      // Compute partial quotient.
          t.c[ HI ] -= *u * v;          // Subtract partial product from dividend.
       }
       else
          *u = 0;                       // MSB of quotient is zero (0).

       if (n > 1)
       {                                // Dividend at least two bytes wide.
          n--;
          do
          {                             // Now *u < v always.
             t.c[ LO ] = *++u;          // t = u-1:u.
             *u = t.i / v;              // u = t / v; Compute partial quotient.
             t.i -= *u * v;             // Subtract partial product from dividend.
             t.c[ HI ] = t.c[ LO ];     // 
          } while (--n);
       }
    }
}

#if EXTRAS
// Shift (in-place) n-byte 'x' to the right by 's' bits.
uint8r_t s_mask[] = { 0x00, 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F };

void shift_right (uint8x_t *x, uint8_t n, uint8_t s)
{                                       // Shift right n-byte number right 's' bits.
    uint8_t carry, old_carry;

    s &= 0x07;
    old_carry = 0x00;

    while (n--)
    {
       carry = *x & s_mask[ s ];
       *x++ = old_carry << (8 - s) | (*x >> s);
       old_carry = carry;
    }
}
#endif // EXTRAS.

#if AUTOCAL && \
    (EQUATION != _1ELEMENT_3WIRE \
     && EQUATION != _2ELEMENT_4WIRE_DELTA \
     && EQUATION != _2ELEMENT_4WIRE_WYE)
// Calculates the absolute (positive) value of 'x'.
void abs_x (uint8x_t *x, uint8x_t *x0, uint8_t n)            
{                                       // Take absolute value of n-byte 'x0' to 'x'.          
    if (*x0 > 0x7F)
    {
        complement_x (x, x0, n);
        add_1 (x, n, 1);
    }
    else
        memcpy_xx (x, x0, n);
}

// Calculates the twos complement value of 'x'.
void complement_x (uint8x_t *x, uint8x_t *x0, uint8_t n)
{                                       // Take ones-complement of n-byte 'x0' to 'x'.
    while (n--)
        *x++ = *x0++ ^ 0xFF;
}
#endif // EXTRAS.

#if EEPROM && I2C_INT
#pragma save
#pragma NOAREGS
// returns the log base 2 of k
uint8_t log2 (uint16_t k) small reentrant
{
    uint8_t l = 0;
    
    if (0 == k)
       return (0xFFFF);

    while (0x0000 == (k & 0x8000))
    {
       k <<= 1;
       l++;
    }       
    return (15 - l);
}
#pragma restore
#endif // EEPROM.

#if EXTRAS
#pragma save
#pragma NOAREGS
// returns the maximum of a and b
uint32_t lmax (uint32_t a, uint32_t b) small reentrant
{
    return (a > b ? a : b);
}
#pragma restore

#pragma save
#pragma NOAREGS
// returns the minimum of a and b
uint32_t lmin (uint32_t a, uint32_t b) small reentrant
{
    return (a < b ? a : b);
}
#pragma restore
#endif // EXTRAS.

#if PROFILE || (PULSE_SOURCE && ABS_VALUE)
#pragma save
#pragma NOAREGS
// returns the absolute (positive) value of 'x'.
int32_t labsx (int32_t x) small reentrant
{
    return (x < 0 ? -x : x);
}
#pragma restore
#endif

#if PHASE_ANGLES
//  Domain of following equation is |tan(theta)| <= 1.
//
//  atan(tan(theta)) = -3/10 + 62 * tan(theta) - 100/6 * tan(theta)^2 degrees.
//  accurate to less than one degree (about 0.1 degree)
//
//  atan(y/x) = -3/10 + 62 * (y/x) - 100/6 * (y/x)^2 degrees.
//            = -3/10 + (y/x) * [62  - 100/6 * (y/x)] degrees.
//            = -3/10 + (y/x) * [62  - 50/3 *  (y/x)] degrees.
//
//  atan(y/x) * 1000  = [62000 - ((y * 1000 / x) * 50 / 3)] * y / x - 300 mDegrees.
//                    = [62000 - ((y * 50000 / x) / 3)] * y / x - 300 mDegrees.
//                    = [62000 -  (y * 16667 / x)] * y / x - 300 mDegrees.
//  
//  Scale y and x so that y < x < 2^16. 
//  
#define NINETY       90000
#define ONE_EIGHTY  180000
#define THREE_SIXTY 360000

uint32_t latan2 (int32_t sy, int32_t sx)
{                                       // Range (0, 360) degrees. 
    bool signY, signX, exchange;
    uint16_t *y, *x;
    uint32_t a;

    if (signY = (sy < 0))
       sy = -sy;
    
    if (signX = (sx < 0))
       sx = -sx;

    if (exchange = (sy > sx))
    {                                   // Exchange values.
       x = (uint16_t *) &sy;    y = (uint16_t *) &sx;
    }
    else
    {
       x = (uint16_t *) &sx;    y = (uint16_t *) &sy;
    }
                                        // Point to MSBs.
    if (0 == *x)
    {
       x++;  y++;                       // Point to 'real' MSBs.
    }
    
    if (0 == *x)
    {
       x++;  y++;                       // Point to 'real' MSBs.
    }
    
    a = (62000 - ((uint32_t) *y * 16667) / *x) * *y / *x - 300;
                                        // [0, 45] degrees.
    if (exchange)
       a = NINETY  - a;                 // (45, 90] degrees.

    if (signX)
       a = ONE_EIGHTY - a;              // (90, 180] degrees.

    if (signY)
       a = THREE_SIXTY - a;             // (0, -180) degrees.
 
    return (a);
}
#endif

#if 0
#if CALIBRATION
//---------------------------------------------------------------------------//
// Returns the 32-bit square root of 32-bit argument 'y'.
// The most  significant two bytes of result are the integer part,
// the least significant two bytes of result are the fractional part.
uint32_t sqrt4_4 (int32_t y)        // Sqrt (y << 32).
{                                       
    #if OUTPUTS_DATA_SPACE == OUTPUTS_IN_IDATA
    uint8_t idata x[8];
    #else
    uint8_t pdata x[8];
    #endif

    if (y < 0)                          // If noise, set to zero.
       return (0);  
    else
    {
       * (uint32i_t *) &x[0] = y;       // x = y scaled by 2^32.
       * (uint32i_t *) &x[4] = 0;       
       return (sqrt8_4 (x));             // Result is sqrt(y) scaled by 2^16.
    }
}
#endif // RMS_VALUES.
#endif

#if 0
#if VA_ELEMENT || VA_SUMS || CALIBRATION
//===========================================================================//
/*
    square = TWOto30;                   // = 2^30.

    square_root  = TWOto30;             // = 1000 0000 0000 0000 * 2^15.
    square += TWOto28 + square_root;    // = 1000 0000 0000 0000 * 2^15.
                                               
    square_root >>= 1;                  // = 0100 0000 0000 0000 * 2^15.
    square_root += TWOto28;             // = 0110 0000 0000 0000 * 2^15.
    square += TWOto26 + square_root;    // = 1100 0000 0000 0000 * 2^14.

    square_root >>= 1;                  // = 0011 0000 0000 0000 * 2^15.
    square_root += TWOto26;             // = 0011 1000 0000 0000 * 2^15.
    square += TWOto24 + square_root;    // = 1110 0000 0000 0000 * 2^13.

    square_root >>= 1;                  // = 0001 1100 0000 0000 * 2^15.
    square_root += TWOto24;             // = 0001 1110 0000 0000 * 2^15.
    square += TWOto22 + square_root;    // = 1111 0000 0000 0000 * 2^12.
    .............................................................................
*/

#define TWOto62 0x40
// Returns the 32-bit square root of the 64-bit argument 'x'.
uint32_t sqrt8_4 (uint8p_t *x) 
{
    uint8_t xdata square[8], square_root[8], square_tmp[8];
    uint8_t xdata TWOto2M[8];
    uint8_t M1;                                             // M + 1.

    memset_x (square, 0x00, sizeof (square));               // square = 0;
    memset_x (square_root, 0x00, sizeof (square_root));
                                                            // square_root = 0;
    if (x[0] >= TWOto62)
    {
       square[0]      = TWOto62;
       square_root[0] = TWOto62;                            // square_root * 2^31.
    }

    TWOto2M[0] = TWOto62;                                   // TWOto2M = 2^62.
    memset_x (&TWOto2M[1], 0x00, sizeof (TWOto2M) - 1);

    M1 = 31;
    do
    {
       memset_x (square_tmp, 0x00, sizeof (square_tmp));    // square_tmp  = 0.
       add8_8 (square_tmp, TWOto2M);                        // square_tmp += TWOto2M. 
       add8_8 (square_tmp, square_root);                    // square_tmp += square_root.
       add8_8 (square_tmp, square);                         // square_tmp += square.
       shift_right8_1 (square_root);                        // square_root >>= 1.
       
       if (memcmp_px (x, square_tmp, sizeof (square_tmp)) >= 0)
       {
          memcpy_xx (square, square_tmp, sizeof (square));
          add8_8 (square_root, TWOto2M);
       }

       shift_right8_2 (TWOto2M);                            // TWOto2M >>= 2.
    } while (--M1);

    shift_right8_1 (square_root);

    return (* (uint32_t *) &square_root[4]);
}

// Shift 64-bit 'x' right 1 bit.
#if MATH_FAST
void shift_right8_1 (uint8x_t *x)       // Shift right 8-byte number right 1-bit.
{
    uint8_t carry_a, carry_b;

    carry_a = *x << 7;      *x =           (*x >> 1);  x++;
    carry_b = *x << 7;      *x = carry_a | (*x >> 1);  x++;
    carry_a = *x << 7;      *x = carry_b | (*x >> 1);  x++;
    carry_b = *x << 7;      *x = carry_a | (*x >> 1);  x++;
    carry_a = *x << 7;      *x = carry_b | (*x >> 1);  x++;
    carry_b = *x << 7;      *x = carry_a | (*x >> 1);  x++;
    carry_a = *x << 7;      *x = carry_b | (*x >> 1);  x++;
    carry_b = *x << 7;      *x = carry_a | (*x >> 1);
}
#else
void shift_right8_1 (uint8x_t *x)       // Shift right 8-byte number right 1-bit.
{
    uint8_t n = 8;
    uint8_t carry_a = 0;
    uint8_t carry_b;

    do
    {
        carry_b = *x << 7;
        *x = carry_a | (*x >> 1);
        carry_a = carry_b;
        x++;
    } while (--n);
}
#endif // MATH_FAST.

// Shift 64-bit 'x' right 2 bits.
#if MATH_FAST