math24lb.h

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

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

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

#ifndef FpFlags_defined
 #error The basic 24 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

float24 log( sharedM float24 arg1f24)
{
    sharedM float24 arg2f24;

    if (arg1f24.mid8 & 0x80)  //  test for negative argument
        goto DOMERR32;
    if (!arg1f24.high8)   //  test for zero argument
        goto DOMERR32;

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

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

    arg2f24 = 1.0;

    //  .70710678118655 = 7E3504F3
    W = arg1f24.low8 - 0x05;
    W = 0x35;
    if (!Carry)
        W = 0x35+1;
    W = arg1f24.mid8 - W;

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

    float24 d = arg1f24;  //  save z

    // POLL132  LOG32Q,2,0  ; Q(z)
    arg1f24 += 0.33339502905E+1; /* LOG32Q1 */
    arg1f24 *= d;
    arg1f24 += 0.24993759223E1;  /* LOG32Q0 */
    float24 c = arg1f24;

     //   minimax rational approximation  z-.5*z*z+z*(z*z*P(z)/Q(z))
    // POL32  LOG32P,1,0  ; P(z)
    arg1f24 = d;
    arg1f24 *= 0.48646956294; /* LOG32P1 */
    arg1f24 += 0.83311400452; /* LOG32P0 */

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

    W = d.high8;  arg1f24.high8 = W;  arg2f24.high8 = W;
    W = d.mid8;   arg1f24.mid8 = W;   arg2f24.mid8 = W;
    W = d.low8;   arg1f24.low8 = W;   arg2f24.low8 = W;

    arg1f24 *= arg2f24;       // z * z;

    float24 e = arg1f24;
    arg1f24 *= c;          //  z*z*P(z)/Q(z)
    arg1f24 *= d;          //  z*(z*z*P(z)/Q(z))

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

    if (!xexp)
        goto RETURN;

    e = arg1f24;  //  save

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

    d = arg1f24;  //  save k

    arg1f24 *= -0.000212194440055;

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

    e = arg1f24;  //  save

    arg1f24 = d * 0.693359375;

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

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

  DOMERR32:
    FpDomainError = 1;   //  domain error

  RETURN:
    return arg1f24;
}



float24 log10( sharedM float24 arg1f24)
{
    sharedM float24 arg2f24;      // allocation 'trick'

    char flags = FpFlags;
    FpFlags |= 0x40;

    arg1f24 = log( arg1f24);

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

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

    return arg1f24;
}



char floorMaskTable24( 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;
}

float24 floor24( sharedM float24 arg1f24)
{
    if (!arg1f24.high8)
        goto RETURN;

    uns16 ma = arg1f24.low16;  //  save mantissa

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

    //  save number of zero bits
    W = 15 - W;

    char tmpa = W;
    tmp = W;

    if (tmp & 0x8)  // LSB+3		; divide by eight
        goto FLOOR24MASKH;


    W = floorMaskTable24( tmpa);    //  get mask
    arg1f24.low8 &= W;
    if (!(arg1f24.mid8 & 0x80))  //  if negative, round down
        goto RETURN;

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

    tmp = ~arg1B7;
    arg1f24.low8 += tmp + 1;
    if (Zero_)
        arg1f24.mid8 += 1;

    //  has rounding caused carryout?
    if (!Zero_)
        goto RETURN;
    arg1f24.mid8 = rr( arg1f24.mid8);
    arg1f24.low8 = rr( arg1f24.low8);

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


  FLOOR24MASKH:
    W = floorMaskTable24( tmpa);  //  get mask
    arg1f24.mid8 &= W;
    arg1f24.low8 = 0;

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

    arg1B7 = W;
    if (( arg1f24.low8 - ma.low8) != 0)
        goto FLOOR24RNDH;
    if (!(arg1f24.mid8 - ma.mid8))
        goto RETURN;

  FLOOR24RNDH:
    tmp = ~arg1B7;
    arg1f24.mid8 += tmp + 1;

    //  has rounding caused carryout?
    if (!Carry)
        goto RETURN;
    arg1f24.mid8 = rr( arg1f24.mid8);
    arg1f24.low8 = rr( arg1f24.low8);

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


  FLOOR24ZERO:
    if (!(arg1f24.mid8 & 0x80))
        goto RES0;
    return -1.0;

  RES0:
    W = 0;
    goto ASSIGNW;

  OVERFLOW:
    FpOverflow = 1;
    W = 0xFF;
  ASSIGNW:
    arg1f24.low8 = W;
    arg1f24.mid8 = W;
    arg1f24.high8 = W;

  RETURN:
    return arg1f24;
}



float24 exp10( sharedM float24 arg1f24)
{
    sharedM float24 arg2f24;      // allocation 'trick'
    float24 c, d;
    char xexp;

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

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

    if (!(arg1f24.mid8 & 0x80))  {
        //  positive domain check
        W = 26 - arg1f24.mid8;
        if (!Carry)
            goto DOMERR;
        if (!Zero_)
            goto ARGOK;

        W = 33 - arg1f24.low8;
        if (!Carry)
            goto DOMERR;
    }
    else  {

        W = 151 - arg1f24.mid8;
        if (!Carry)
            goto DOMERR;
        if (!Zero_)
            goto ARGOK;

        W = 184 - arg1f24.low8;
        if (!Carry)
            goto DOMERR;
    }

  ARGOK:

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

    c = arg1f24;  //  save x
    arg1f24 *= 3.32192809489;  //  1/log10(2) = 3.32192809489
    arg1f24 += 0.5;  //  k = [ x / log10(2) + .5 ]
    arg1f24 = floor24( arg1f24);

    float24 e = arg1f24; //  save float k
    FpFlags &= ~0x40;
    //floating point to integer conversion
    xexp = arg1f24; //  k = [ x / ln(2) + .5 ]
    FpFlags |= 0x40;
    arg1f24 = e * -0.30078125; //  c1
    d = arg1f24 + c;
    arg1f24 = e * -2.487456637421670e-04;  // c2
    arg1f24 += d;
    d = arg1f24;  //  save f

    if (!(d.mid8 & 0x80))  {
        // POL32  EXP1032H,5,4   ; minimax approximation on [0,log10(2)/2]
        arg1f24 *= 6.388992868121E-1;/* EXP1032H5 */
        arg1f24 += 1.154596329197E0; /* EXP1032H4 */
        arg1f24 *= d;
        arg1f24 += 2.035920309947E0; /* EXP1032H3 */