math32lb.h

FreeRTOS 是一个源码公开的免费的嵌入式实时操作系统

// *************************************************
// 32 bit floating point math functions
// Copyright (c) B Knudsen Data, Norway, 2000 - 2005
// *************************************************

#pragma library 1
/* PROTOTYPES for page definition in application header file:
float32 log( float32 arg1f32);
float32 log10( float32 arg1f32);
float32 exp10( float32 arg1f32);
float32 exp( float32 arg1f32);
float32 cos( float32 arg1f32);
float32 sin( float32 arg1f32);
float32 sqrt( float32 arg1f32);
*/

#ifndef FpFlags_defined
 #error The basic 32 bit floating point math library must be included first
#endif

#if __CoreSet__ / 100 == 12
 #error Math functions (exp,log,..) are not adapted to 12 bit core devices
#endif


float32 log( sharedM float32 arg1f32)
{
    sharedM float32 arg2f32;

    if (arg1f32.midH8 & 0x80)  // test for negative argument
        goto DOMERR;
    if (!arg1f32.high8)   // test for zero argument
        goto DOMERR;

    char savedFlags = FpFlags;   //  save rounding flag
    FpFlags |= 0x40;  //  enable rounding

    char xexp = arg1f32.high8 - (FpBIAS-1);
    arg1f32.high8 = FpBIAS-1;

    arg2f32 = 1.0;

    //  .70710678118655 = 7E3504F3
    W = arg1f32.low8 - 0xF3;
    W = 0x04;
    if (!Carry)
        W = 0x04+1;
    W = arg1f32.midL8 - W;
    W = 0x35;
    if (!Carry)
        W = 0x35+1;
    W = arg1f32.midH8 - W;

    if (Carry)
        arg1f32 -= arg2f32;
    else  {
        arg1f32.high8 += 1;  /* arg1f32 *= 2; */
        arg1f32 -= arg2f32;
        xexp -= 1;
    }

    float32 d = arg1f32;  //  save z

    // Q(z)
    arg1f32 += 0.33339502905E+1; /* LOG32Q1 */
    arg1f32 *= d;
    arg1f32 += 0.24993759223E1;  /* LOG32Q0 */
    float32 c = arg1f32;

    // minimax rational approximation  z-.5*z*z+z*(z*z*P(z)/Q(z))
    // P(z)
    arg1f32 = d;
    arg1f32 *= 0.48646956294; /* LOG32P1 */
    arg1f32 += 0.83311400452; /* LOG32P0 */

    c = arg1f32 / c;   //  P(z)/Q(z)

    W = d.high8;  arg1f32.high8 = W;  arg2f32.high8 = W;
    W = d.midH8;  arg1f32.midH8 = W;  arg2f32.midH8 = W;
    W = d.midL8;  arg1f32.midL8 = W;  arg2f32.midL8 = W;
    W = d.low8;   arg1f32.low8 = W;   arg2f32.low8 = W;

    arg1f32 *= arg2f32;       // z * z;

    float32 e = arg1f32;
    arg1f32 *= c;          //  z*z*P(z)/Q(z)
    arg1f32 *= d;          //  z*(z*z*P(z)/Q(z))

    arg2f32 = e;
    if (arg2f32.high8)
        arg2f32.high8 --;  // arg2f32 *= 0.5;
    arg1f32 -= arg2f32;       //  -.5*z*z + z*(z*z*P(z)/Q(z))
    arg1f32 += d;          //  z -.5*z*z + z*(z*z*P(z)/Q(z))

    if (!xexp)
        goto RETURN;

    e = arg1f32;  //  save

    // integer to floating point conversion
    arg1f32 = (int8) xexp;

    d = arg1f32;  //  save k

    arg1f32 *= -0.000212194440055;

    arg1f32 += e;   //  log(1+z) + k*log(2)

    e = arg1f32;  //  save

    arg1f32 = d * 0.693359375;

    arg1f32 += e;      //  log(1+z) + k*log(2)

    if (!(savedFlags & 0x40))
        FpFlags &= ~0x40;   //  restore rounding flag
    goto RETURN;

  DOMERR:
    FpDomainError = 1;   //  domain error

  RETURN:
    return arg1f32;
}



float32 log10( sharedM float32 arg1f32)
{
    sharedM float32 arg2f32;      // allocation 'trick'

    char flags = FpFlags;
    FpFlags |= 0x40;

    arg1f32 = log( arg1f32);

    arg1f32 *= 0.43429448190325;  //  log10(e);

    if (!(flags & 0x40))
        FpFlags &= ~0x40;

    return arg1f32;
}





char floorMaskTable32( char i)
{
    if (i & 4)  {
        if (i & 2)  {
            if (i & 1)
                return 128;
            return 192;
        }
        if (i & 1)
            return 224;
        return 240;
    }
    if (i & 2)  {
        if (i & 1)
            return 248;
        return 252;
    }
    if (i & 1)
        return 254;
    return 255;
}

float32 floor32( sharedM float32 arg1f32)
{
    if (!arg1f32.high8)  //  test for zero argument
        goto RETURN;

    uns24 ma = arg1f32.low24;  //  save mantissa

    W = arg1f32.high8 - 127;
    char tmp = W;      // OPM
    if (tmp & 0x80)
        goto FLOOR32ZERO;

    //  save number of zero bits in TEMP.midH8
    W = 23 - W;

    char tmpa = W;
    tmp = W;

    if (tmp & 0x10)  // LSB+1+3 ; divide by eight
        goto FLOOR32MASKH;

    if (tmp & 0x8)  // LSB+3
        goto FLOOR32MASKM;


    W = floorMaskTable32( tmpa);    //  get mask
    arg1f32.low8 &= W;
    if (!(arg1f32.midH8 & 0x80))  //  if negative, round down
        goto RETURN;

    char arg1B7 = W;
    if (!(arg1f32.low8 - ma.low8))
        goto RETURN;

    tmp = ~arg1B7;
    arg1f32.low8 += tmp + 1;
    if (Zero_)
        arg1f32.midL8 += 1;
    if (Zero_)
        arg1f32.midH8 += 1;

    //  has rounding caused carryout?
    if (!Zero_)
        goto RETURN;
    arg1f32.midH8 = rr( arg1f32.midH8);
    arg1f32.midL8 = rr( arg1f32.midL8);
    arg1f32.low8 = rr( arg1f32.low8);

    //  check for overflow
    arg1f32.high8 = incsz( arg1f32.high8);
    goto RETURN;
    goto OVERFLOW;


  FLOOR32MASKM:
    W = floorMaskTable32( tmpa);  //  get mask
    arg1f32.midL8 &= W;
    arg1f32.low8 = 0;

    //  if negative, round down
    if (!(arg1f32.midH8 & 0x80))
        goto RETURN;
    arg1B7 = W;
    if (( arg1f32.low8 - ma.low8) != 0)
        goto FLOOR32RNDM;
    if (!(arg1f32.midL8 - ma.midL8))
        goto RETURN;

  FLOOR32RNDM:
    tmp = ~arg1B7;
    arg1f32.midL8 += tmp + 1;
    if (Zero_)
        arg1f32.midH8 += 1;

    //  has rounding caused carryout?
    if (!Zero_)
        goto RETURN;
    arg1f32.midH8 = rr( arg1f32.midH8);
    arg1f32.midL8 = rr( arg1f32.midL8);
    arg1f32.low8 = rr( arg1f32.low8);

    //  check for overflow
    arg1f32.high8 = incsz( arg1f32.high8);
    goto RETURN;
    goto OVERFLOW;


  FLOOR32MASKH:
    W = floorMaskTable32( tmpa);    //  get mask
    arg1f32.midH8 &= W;
    arg1f32.midL8 = 0;
    arg1f32.low8 = 0;

    //  if negative, round down
    if (!(arg1f32.midH8 & 0x80))
        goto RETURN;

    arg1B7 = W;
    if (( arg1f32.low8 - ma.low8) != 0)
        goto FLOOR32RNDH;
    if (( arg1f32.midL8 - ma.midL8) != 0)
        goto FLOOR32RNDH;
    if (!(arg1f32.midH8 - ma.midH8))
        goto RETURN;

  FLOOR32RNDH:
    tmp = ~arg1B7;
    arg1f32.midH8 += tmp + 1;

    //  has rounding caused carryout?
    if (!Carry)
        goto RETURN;
    arg1f32.midH8 = rr( arg1f32.midH8);
    arg1f32.midL8 = rr( arg1f32.midL8);

    //  check for overflow
    arg1f32.high8 = incsz( arg1f32.high8);
    goto RETURN;
    goto OVERFLOW;



  FLOOR32ZERO:
    if (!(arg1f32.midH8 & 0x80))
        goto RES0;
    return -1.0;

  RES0:
    W = 0;
    goto ASSIGNW;

  OVERFLOW:
    FpOverflow = 1;
    W = 0xFF;
  ASSIGNW:
    arg1f32.low8 = W;
    arg1f32.midL8 = W;
    arg1f32.midH8 = W;
    arg1f32.high8 = W;

  RETURN:
    return arg1f32;
}


float32 exp10( sharedM float32 arg1f32)
//  Maximum argument :  38.531839445 = log10(2**128)
//  Minimum argument : -37.9297794537 = log10(2**-126)
{
    sharedM float32 arg2f32;      // allocation 'trick'
    float32 c, d;
    char xexp;

    if (( arg1f32.high8 - 92) & 0x80)
        goto EXP1;   //  return e**x = 1

    W = 132 - arg1f32.high8;
    if (!Carry)
        goto DOMERR;
    if (!Zero_)
        goto ARGOK;

    if (!(arg1f32.midH8 & 0x80))  {
        //  positive domain check

        W = 26 - arg1f32.midH8;
        if (!Carry)
            goto DOMERR;
        if (!Zero_)
            goto ARGOK;

        W = 32 - arg1f32.midL8;
        if (!Carry)
            goto DOMERR;
        if (!Zero_)
            goto ARGOK;

        W = 155 - arg1f32.low8;
        if (!Carry)
            goto DOMERR;
    }
    else  {
        //  negative domain check
        W = 151 - arg1f32.midH8;
        if (!Carry)
            goto DOMERR;
        if (!Zero_)
            goto ARGOK;

        W = 184 - arg1f32.midL8;
        if (!Carry)
            goto DOMERR;
        if (!Zero_)
            goto ARGOK;

        W = 24 - arg1f32.low8;
        if (!Carry)
            goto DOMERR;
    }

   ARGOK:

    char savedFlags = FpFlags;
    FpFlags |= 0x40;  //  enable rounding

    // Range reduction