CCS PCM C Compiler, Version 4.106, 47914               17-V-13 12:14

               Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst

               ROM used: 799 words (10%)
                         Largest free fragment is 2048
               RAM used: 28 (8%) at main() level
                         38 (10%) worst case
               Stack:    2 locations

*
0000:  MOVLW  02
0001:  MOVWF  0A
0002:  GOTO   266
0003:  NOP
.................... #include "main.h" 
.................... #include <16F887.h> 
.................... //////// Standard Header file for the PIC16F887 device //////////////// 
.................... #device PIC16F887 
.................... #list 
....................  
.................... #device adc=8 
....................  
.................... #FUSES NOWDT                    //No Watch Dog Timer 
.................... #FUSES INTRC                    //Internal RC Osc 
.................... #FUSES NOPUT                    //No Power Up Timer 
.................... #FUSES MCLR                     //Master Clear pin enabled 
.................... #FUSES NOPROTECT                //Code not protected from reading 
.................... #FUSES NOCPD                    //No EE protection 
.................... #FUSES NOBROWNOUT               //No brownout reset 
.................... #FUSES IESO                     //Internal External Switch Over mode enabled 
.................... #FUSES FCMEN                    //Fail-safe clock monitor enabled 
.................... #FUSES NOLVP                    //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O 
.................... #FUSES NODEBUG                  //No Debug mode for ICD 
.................... #FUSES NOWRT                    //Program memory not write protected 
.................... #FUSES BORV40                   //Brownout reset at 4.0V 
....................  
.................... #use delay(clock=8000000) 
*
0250:  MOVLW  37
0251:  MOVWF  04
0252:  BCF    03.7
0253:  MOVF   00,W
0254:  BTFSC  03.2
0255:  GOTO   263
0256:  MOVLW  02
0257:  MOVWF  78
0258:  CLRF   77
0259:  DECFSZ 77,F
025A:  GOTO   259
025B:  DECFSZ 78,F
025C:  GOTO   258
025D:  MOVLW  97
025E:  MOVWF  77
025F:  DECFSZ 77,F
0260:  GOTO   25F
0261:  DECFSZ 00,F
0262:  GOTO   256
0263:  BCF    0A.3
0264:  BCF    0A.4
0265:  GOTO   31D (RETURN)
.................... #use i2c(Master,Slow,sda=PIN_C4,scl=PIN_C3) 
*
0089:  MOVLW  08
008A:  MOVWF  78
008B:  NOP
008C:  BCF    07.3
008D:  BCF    20.3
008E:  MOVF   20,W
008F:  BSF    03.5
0090:  MOVWF  07
0091:  NOP
0092:  BCF    03.5
0093:  RLF    3D,F
0094:  BCF    07.4
0095:  BTFSS  03.0
0096:  GOTO   09D
0097:  BSF    20.4
0098:  MOVF   20,W
0099:  BSF    03.5
009A:  MOVWF  07
009B:  GOTO   0A1
009C:  BCF    03.5
009D:  BCF    20.4
009E:  MOVF   20,W
009F:  BSF    03.5
00A0:  MOVWF  07
00A1:  NOP
00A2:  BCF    03.5
00A3:  BSF    20.3
00A4:  MOVF   20,W
00A5:  BSF    03.5
00A6:  MOVWF  07
00A7:  BCF    03.5
00A8:  BTFSS  07.3
00A9:  GOTO   0A8
00AA:  DECFSZ 78,F
00AB:  GOTO   08B
00AC:  NOP
00AD:  BCF    07.3
00AE:  BCF    20.3
00AF:  MOVF   20,W
00B0:  BSF    03.5
00B1:  MOVWF  07
00B2:  NOP
00B3:  BCF    03.5
00B4:  BSF    20.4
00B5:  MOVF   20,W
00B6:  BSF    03.5
00B7:  MOVWF  07
00B8:  NOP
00B9:  NOP
00BA:  BCF    03.5
00BB:  BSF    20.3
00BC:  MOVF   20,W
00BD:  BSF    03.5
00BE:  MOVWF  07
00BF:  BCF    03.5
00C0:  BTFSS  07.3
00C1:  GOTO   0C0
00C2:  CLRF   78
00C3:  NOP
00C4:  BTFSC  07.4
00C5:  BSF    78.0
00C6:  BCF    07.3
00C7:  BCF    20.3
00C8:  MOVF   20,W
00C9:  BSF    03.5
00CA:  MOVWF  07
00CB:  BCF    03.5
00CC:  BCF    07.4
00CD:  BCF    20.4
00CE:  MOVF   20,W
00CF:  BSF    03.5
00D0:  MOVWF  07
00D1:  BCF    03.5
00D2:  RETURN
*
010C:  MOVLW  08
010D:  MOVWF  3E
010E:  MOVF   77,W
010F:  MOVWF  3F
0110:  BSF    20.4
0111:  MOVF   20,W
0112:  BSF    03.5
0113:  MOVWF  07
0114:  NOP
0115:  BCF    03.5
0116:  BSF    20.3
0117:  MOVF   20,W
0118:  BSF    03.5
0119:  MOVWF  07
011A:  BCF    03.5
011B:  BTFSS  07.3
011C:  GOTO   11B
011D:  BTFSC  07.4
011E:  BSF    03.0
011F:  BTFSS  07.4
0120:  BCF    03.0
0121:  RLF    78,F
0122:  NOP
0123:  BCF    20.3
0124:  MOVF   20,W
0125:  BSF    03.5
0126:  MOVWF  07
0127:  BCF    03.5
0128:  BCF    07.3
0129:  DECFSZ 3E,F
012A:  GOTO   110
012B:  BSF    20.4
012C:  MOVF   20,W
012D:  BSF    03.5
012E:  MOVWF  07
012F:  NOP
0130:  BCF    03.5
0131:  BCF    07.4
0132:  MOVF   3F,W
0133:  BTFSC  03.2
0134:  GOTO   13A
0135:  BCF    20.4
0136:  MOVF   20,W
0137:  BSF    03.5
0138:  MOVWF  07
0139:  BCF    03.5
013A:  NOP
013B:  BSF    20.3
013C:  MOVF   20,W
013D:  BSF    03.5
013E:  MOVWF  07
013F:  BCF    03.5
0140:  BTFSS  07.3
0141:  GOTO   140
0142:  NOP
0143:  BCF    07.3
0144:  BCF    20.3
0145:  MOVF   20,W
0146:  BSF    03.5
0147:  MOVWF  07
0148:  NOP
0149:  BCF    03.5
014A:  BCF    07.4
014B:  BCF    20.4
014C:  MOVF   20,W
014D:  BSF    03.5
014E:  MOVWF  07
014F:  BCF    03.5
0150:  RETURN
.................... #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD 
*
001F:  BCF    20.7
0020:  MOVF   20,W
0021:  BSF    03.5
0022:  MOVWF  07
0023:  BCF    03.5
0024:  BCF    07.7
0025:  MOVLW  08
0026:  MOVWF  78
0027:  GOTO   028
0028:  NOP
0029:  BSF    78.7
002A:  GOTO   039
002B:  BCF    78.7
002C:  RRF    40,F
002D:  BTFSC  03.0
002E:  BSF    07.7
002F:  BTFSS  03.0
0030:  BCF    07.7
0031:  BSF    78.6
0032:  GOTO   039
0033:  BCF    78.6
0034:  DECFSZ 78,F
0035:  GOTO   02C
0036:  GOTO   037
0037:  NOP
0038:  BSF    07.7
0039:  MOVLW  3F
003A:  MOVWF  04
003B:  DECFSZ 04,F
003C:  GOTO   03B
003D:  NOP
003E:  BTFSC  78.7
003F:  GOTO   02B
0040:  BTFSC  78.6
0041:  GOTO   033
0042:  RETURN
....................  
....................  
.................... #include "HMC5883L.h" 
.................... // i2c slave addresses 
.................... #define HMC5883L_WRT_ADDR  0x3C 
.................... #define HMC5883L_READ_ADDR 0x3D 
....................  
.................... // Register addresses 
.................... #define HMC5883L_CFG_A_REG 0x00 
.................... #define HMC5883L_CFG_B_REG 0x01 
.................... #define HMC5883L_MODE_REG  0x02 
.................... #define HMC5883L_X_MSB_REG 0x03 
....................  
.................... //Konstanty nastavujici rozsah 
.................... //pro void set_mag_roz (unsigned int8 h) 
.................... #define MAG_ROZ088  0x00 
.................... #define MAG_ROZ130  0x20 
.................... #define MAG_ROZ190  0x40 
.................... #define MAG_ROZ250  0x60 
.................... #define MAG_ROZ400  0x80 
.................... #define MAG_ROZ470  0xA0 
.................... #define MAG_ROZ560  0xC0 
.................... #define MAG_ROZ810  0xE0 
....................  
....................  
.................... #include "HMC5883L.c" 
.................... //------------------------------ 
.................... // Low level routines 
.................... //------------------------------ 
.................... void hmc5883l_write_reg(int8 reg, int8 data) 
.................... { 
.................... i2c_start(); 
*
00D3:  BSF    20.4
00D4:  MOVF   20,W
00D5:  BSF    03.5
00D6:  MOVWF  07
00D7:  NOP
00D8:  BCF    03.5
00D9:  BSF    20.3
00DA:  MOVF   20,W
00DB:  BSF    03.5
00DC:  MOVWF  07
00DD:  NOP
00DE:  BCF    03.5
00DF:  BCF    07.4
00E0:  BCF    20.4
00E1:  MOVF   20,W
00E2:  BSF    03.5
00E3:  MOVWF  07
00E4:  NOP
00E5:  BCF    03.5
00E6:  BCF    07.3
00E7:  BCF    20.3
00E8:  MOVF   20,W
00E9:  BSF    03.5
00EA:  MOVWF  07
.................... i2c_write(HMC5883L_WRT_ADDR); 
00EB:  MOVLW  3C
00EC:  BCF    03.5
00ED:  MOVWF  3D
00EE:  CALL   089
.................... i2c_write(reg); 
00EF:  MOVF   37,W
00F0:  MOVWF  3D
00F1:  CALL   089
.................... i2c_write(data); 
00F2:  MOVF   38,W
00F3:  MOVWF  3D
00F4:  CALL   089
.................... i2c_stop(); 
00F5:  BCF    20.4
00F6:  MOVF   20,W
00F7:  BSF    03.5
00F8:  MOVWF  07
00F9:  NOP
00FA:  BCF    03.5
00FB:  BSF    20.3
00FC:  MOVF   20,W
00FD:  BSF    03.5
00FE:  MOVWF  07
00FF:  BCF    03.5
0100:  BTFSS  07.3
0101:  GOTO   100
0102:  NOP
0103:  GOTO   104
0104:  NOP
0105:  BSF    20.4
0106:  MOVF   20,W
0107:  BSF    03.5
0108:  MOVWF  07
0109:  NOP
.................... } 
010A:  BCF    03.5
010B:  RETURN
....................  
.................... //------------------------------ 
.................... int8 hmc5883l_read_reg(int8 reg) 
.................... { 
.................... int8 retval; 
....................  
.................... i2c_start(); 
.................... i2c_write(HMC5883L_WRT_ADDR); 
.................... i2c_write(reg); 
.................... i2c_start(); 
.................... i2c_write(HMC5883L_READ_ADDR); 
.................... retval = i2c_read(0); 
.................... i2c_stop(); 
....................  
.................... return(retval); 
.................... } 
....................  
.................... //------------------------------ 
.................... typedef struct 
.................... { 
.................... signed int16 x; 
.................... signed int16 y; 
.................... signed int16 z; 
.................... }hmc5883l_result; 
....................  
.................... // This global structure holds the values read 
.................... // from the HMC5883L x,y,z registers. 
.................... hmc5883l_result compass = {0,0,0}; 
*
0287:  CLRF   21
0288:  CLRF   22
0289:  CLRF   23
028A:  CLRF   24
028B:  CLRF   25
028C:  CLRF   26
....................  
.................... //------------------------------ 
.................... void hmc5883l_read_data(void) 
.................... { 
.................... unsigned int8 x_lsb; 
.................... unsigned int8 x_msb; 
....................  
.................... unsigned int8 y_lsb; 
.................... unsigned int8 y_msb; 
....................  
.................... unsigned int8 z_lsb; 
.................... unsigned int8 z_msb; 
....................  
.................... i2c_start(); 
*
0151:  BSF    20.4
0152:  MOVF   20,W
0153:  BSF    03.5
0154:  MOVWF  07
0155:  NOP
0156:  BCF    03.5
0157:  BSF    20.3
0158:  MOVF   20,W
0159:  BSF    03.5
015A:  MOVWF  07
015B:  NOP
015C:  BCF    03.5
015D:  BCF    07.4
015E:  BCF    20.4
015F:  MOVF   20,W
0160:  BSF    03.5
0161:  MOVWF  07
0162:  NOP
0163:  BCF    03.5
0164:  BCF    07.3
0165:  BCF    20.3
0166:  MOVF   20,W
0167:  BSF    03.5
0168:  MOVWF  07
.................... i2c_write(HMC5883L_WRT_ADDR); 
0169:  MOVLW  3C
016A:  BCF    03.5
016B:  MOVWF  3D
016C:  CALL   089
.................... i2c_write(HMC5883L_X_MSB_REG);  // Point to X-msb register 
016D:  MOVLW  03
016E:  MOVWF  3D
016F:  CALL   089
.................... i2c_start(); 
0170:  BSF    20.4
0171:  MOVF   20,W
0172:  BSF    03.5
0173:  MOVWF  07
0174:  NOP
0175:  BCF    03.5
0176:  BSF    20.3
0177:  MOVF   20,W
0178:  BSF    03.5
0179:  MOVWF  07
017A:  NOP
017B:  BCF    03.5
017C:  BTFSS  07.3
017D:  GOTO   17C
017E:  BCF    07.4
017F:  BCF    20.4
0180:  MOVF   20,W
0181:  BSF    03.5
0182:  MOVWF  07
0183:  NOP
0184:  BCF    03.5
0185:  BCF    07.3
0186:  BCF    20.3
0187:  MOVF   20,W
0188:  BSF    03.5
0189:  MOVWF  07
.................... i2c_write(HMC5883L_READ_ADDR); 
018A:  MOVLW  3D
018B:  BCF    03.5
018C:  MOVWF  3D
018D:  CALL   089
....................  
.................... x_msb = i2c_read(); 
018E:  MOVLW  01
018F:  MOVWF  77
0190:  CALL   10C
0191:  MOVF   78,W
0192:  MOVWF  38
.................... x_lsb = i2c_read(); 
0193:  MOVLW  01
0194:  MOVWF  77
0195:  CALL   10C
0196:  MOVF   78,W
0197:  MOVWF  37
....................  
.................... z_msb = i2c_read(); 
0198:  MOVLW  01
0199:  MOVWF  77
019A:  CALL   10C
019B:  MOVF   78,W
019C:  MOVWF  3C
.................... z_lsb = i2c_read();    
019D:  MOVLW  01
019E:  MOVWF  77
019F:  CALL   10C
01A0:  MOVF   78,W
01A1:  MOVWF  3B
....................  
.................... y_msb = i2c_read(); 
01A2:  MOVLW  01
01A3:  MOVWF  77
01A4:  CALL   10C
01A5:  MOVF   78,W
01A6:  MOVWF  3A
.................... y_lsb = i2c_read(0); // do a NACK on last read 
01A7:  CLRF   77
01A8:  CALL   10C
01A9:  MOVF   78,W
01AA:  MOVWF  39
....................  
.................... i2c_stop(); 
01AB:  BCF    20.4
01AC:  MOVF   20,W
01AD:  BSF    03.5
01AE:  MOVWF  07
01AF:  NOP
01B0:  BCF    03.5
01B1:  BSF    20.3
01B2:  MOVF   20,W
01B3:  BSF    03.5
01B4:  MOVWF  07
01B5:  BCF    03.5
01B6:  BTFSS  07.3
01B7:  GOTO   1B6
01B8:  NOP
01B9:  GOTO   1BA
01BA:  NOP
01BB:  BSF    20.4
01BC:  MOVF   20,W
01BD:  BSF    03.5
01BE:  MOVWF  07
01BF:  NOP
....................   
.................... // Combine high and low bytes into 16-bit values. 
.................... compass.x = make16(x_msb, x_lsb); 
01C0:  BCF    03.5
01C1:  MOVF   38,W
01C2:  MOVWF  22
01C3:  MOVF   37,W
01C4:  MOVWF  21
.................... compass.y = make16(y_msb, y_lsb); 
01C5:  MOVF   3A,W
01C6:  MOVWF  24
01C7:  MOVF   39,W
01C8:  MOVWF  23
.................... compass.z = make16(z_msb, z_lsb); 
01C9:  MOVF   3C,W
01CA:  MOVWF  26
01CB:  MOVF   3B,W
01CC:  MOVWF  25
.................... } 
01CD:  BCF    0A.3
01CE:  BCF    0A.4
01CF:  GOTO   2F6 (RETURN)
....................  
....................  
....................  
....................  
.................... #include <math.h> 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... ////        (C) Copyright 1996,2008 Custom Computer Services            //// 
.................... //// This source code may only be used by licensed users of the CCS C   //// 
.................... //// compiler.  This source code may only be distributed to other       //// 
.................... //// licensed users of the CCS C compiler.  No other use, reproduction  //// 
.................... //// or distribution is permitted without written permission.           //// 
.................... //// Derivative programs created using this software in object code     //// 
.................... //// form are not restricted in any way.                                //// 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... ////                                                                    //// 
.................... //// History:                                                           //// 
.................... ////  * 9/20/2001 :  Improvments are made to sin/cos code.              //// 
.................... ////                 The code now is small, much faster,                //// 
.................... ////                 and more accurate.                                 //// 
.................... ////  * 2/21/2007 :  Compiler handles & operator differently and does 
.................... ////                 not return generic (int8 *) so type cast is done   //// 
.................... ////                                                                    //// 
.................... //////////////////////////////////////////////////////////////////////////// 
....................  
.................... #ifndef MATH_H 
.................... #define MATH_H 
....................  
.................... #ifdef PI 
.................... #undef  PI 
.................... #endif 
.................... #define PI     3.1415926535897932 
....................  
....................  
.................... #define SQRT2  1.4142135623730950 
....................  
.................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279}; 
.................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718}; 
....................  
.................... ///////////////////////////// Round Functions ////////////////////////////// 
....................  
.................... float32 CEIL_FLOOR(float32 x, unsigned int8 n) 
.................... { 
....................    float32 y, res; 
....................    unsigned int16 l; 
....................    int1 s; 
....................  
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y <= 32768.0) 
....................   res = (float32)(unsigned int16)y; 
....................  
....................  else if (y < 10000000.0) 
....................    { 
....................   l = (unsigned int16)(y/32768.0); 
....................       y = 32768.0*(y/32768.0 - (float32)l); 
....................   res = 32768.0*(float32)l; 
....................   res += (float32)(unsigned int16)y; 
....................  } 
....................  
....................  else 
....................   res = y; 
....................  
....................  y = y - (float32)(unsigned int16)y; 
....................  
....................  if (s) 
....................   res = -res; 
....................  
....................  if (y != 0) 
....................  { 
....................   if (s == 1 && n == 0) 
....................    res -= 1.0; 
....................  
....................   if (s == 0 && n == 1) 
....................    res += 1.0; 
....................  } 
....................  if (x == 0) 
....................     res = 0; 
....................  
....................  return (res); 
.................... } 
....................  
.................... // Overloaded Functions to take care for new Data types in PCD 
.................... // Overloaded function CEIL_FLOOR() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 CEIL_FLOOR(float48 x, unsigned int8 n) 
.................... { 
....................    float48 y, res; 
....................    unsigned int16 l; 
....................    int1 s; 
....................  
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y <= 32768.0) 
....................   res = (float48)(unsigned int16)y; 
....................  
....................  else if (y < 10000000.0) 
....................    { 
....................   l = (unsigned int16)(y/32768.0); 
....................       y = 32768.0*(y/32768.0 - (float48)l); 
....................   res = 32768.0*(float32)l; 
....................   res += (float48)(unsigned int16)y; 
....................  } 
....................  
....................  else 
....................   res = y; 
....................  
....................  y = y - (float48)(unsigned int16)y; 
....................  
....................  if (s) 
....................   res = -res; 
....................  
....................  if (y != 0) 
....................  { 
....................   if (s == 1 && n == 0) 
....................    res -= 1.0; 
....................  
....................   if (s == 0 && n == 1) 
....................    res += 1.0; 
....................  } 
....................  if (x == 0) 
....................     res = 0; 
....................  
....................  return (res); 
.................... } 
....................  
....................  
.................... // Overloaded function CEIL_FLOOR() for data type - Float64 
.................... float64 CEIL_FLOOR(float64 x, unsigned int8 n) 
.................... { 
....................    float64 y, res; 
....................    unsigned int16 l; 
....................    int1 s; 
....................  
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y <= 32768.0) 
....................   res = (float64)(unsigned int16)y; 
....................  
....................  else if (y < 10000000.0) 
....................    { 
....................   l = (unsigned int16)(y/32768.0); 
....................       y = 32768.0*(y/32768.0 - (float64)l); 
....................   res = 32768.0*(float64)l; 
....................   res += (float64)(unsigned int16)y; 
....................  } 
....................  
....................  else 
....................   res = y; 
....................  
....................  y = y - (float64)(unsigned int16)y; 
....................  
....................  if (s) 
....................   res = -res; 
....................  
....................  if (y != 0) 
....................  { 
....................   if (s == 1 && n == 0) 
....................    res -= 1.0; 
....................  
....................   if (s == 0 && n == 1) 
....................    res += 1.0; 
....................  } 
....................  if (x == 0) 
....................     res = 0; 
....................  
....................  return (res); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float floor(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : rounds down the number x. 
.................... // Date : N/A 
.................... // 
.................... float32 floor(float32 x) 
.................... { 
....................    return CEIL_FLOOR(x, 0); 
.................... } 
.................... // Following 2 functions are overloaded functions of floor() for PCD 
.................... // Overloaded function floor() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 floor(float48 x) 
.................... { 
....................    return CEIL_FLOOR(x, 0); 
.................... } 
....................  
.................... // Overloaded function floor() for data type - Float64 
.................... float64 floor(float64 x) 
.................... { 
....................    return CEIL_FLOOR(x, 0); 
.................... } 
.................... #endif 
....................  
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float ceil(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : rounds up the number x. 
.................... // Date : N/A 
.................... // 
.................... float32 ceil(float32 x) 
.................... { 
....................    return CEIL_FLOOR(x, 1); 
.................... } 
.................... // Following 2 functions are overloaded functions of ceil() for PCD 
.................... // Overloaded function ceil() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 ceil(float48 x) 
.................... { 
....................    return CEIL_FLOOR(x, 1); 
.................... } 
....................  
.................... // Overloaded function ceil() for data type - Float64 
.................... float64 ceil(float64 x) 
.................... { 
....................    return CEIL_FLOOR(x, 1); 
.................... } 
.................... #endif 
....................  
....................  //////////////////////////////////////////////////////////////////////////// 
.................... //   float fabs(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the absolute value of floating point number x 
.................... // Returns : returns the absolute value of x 
.................... // Date : N/A 
.................... // 
.................... #define fabs abs 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float fmod(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the floating point remainder of x/y 
.................... // Returns : returns the value of x= i*y, for some integer i such that, if y 
.................... // is non zero, the result has the same isgn of x na dmagnitude less than the 
.................... // magnitude of y. If y is zero then a domain error occurs. 
.................... // Date : N/A 
.................... // 
....................  
.................... float fmod(float32 x,float32 y) 
.................... { 
....................    float32 i; 
....................    if (y!=0.0) 
....................    { 
....................       i=(x/y < 0.0)? ceil(x/y): floor(x/y); 
....................       return(x-(i*y)); 
....................    } 
....................    else 
....................    { 
....................    #ifdef _ERRNO 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    } 
.................... } 
.................... //Overloaded function for fmod() for PCD 
.................... // Overloaded function fmod() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 fmod(float48 x,float48 y) 
.................... { 
....................    float48 i; 
....................    if (y!=0.0) 
....................    { 
....................       i=(x/y < 0.0)? ceil(x/y): floor(x/y); 
....................       return(x-(i*y)); 
....................    } 
....................    else 
....................    { 
....................    #ifdef _ERRNO 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    } 
.................... } 
.................... // Overloaded function fmod() for data type - Float64 
.................... float64 fmod(float64 x,float64 y) 
.................... { 
....................    float64 i; 
....................    if (y!=0.0) 
....................    { 
....................       i=(x/y < 0.0)? ceil(x/y): floor(x/y); 
....................       return(x-(i*y)); 
....................    } 
....................    else 
....................    { 
....................    #ifdef _ERRNO 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    } 
.................... } 
.................... #endif 
.................... //////////////////// Exponential and logarithmic functions //////////////////// 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float exp(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the value (e^x) 
.................... // Date : N/A 
.................... // 
.................... #define LN2 0.6931471805599453 
....................  
.................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, 
....................                      0.0554965651,  0.240227138,  0.693147172}; 
....................  
....................  
.................... float32 exp(float32 x) 
.................... { 
....................    float32 y, res, r; 
....................    #if defined(__PCD__) 
....................    int8 data1; 
....................    #endif 
....................    signed int8 n; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x > 88.722838) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    n = (signed int16)(x/LN2); 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       n = -n; 
....................       y = -y; 
....................    } 
....................  
....................    res = 0.0; 
.................... #if !defined(__PCD__) 
....................    *((unsigned int8 *)(&res)) = n + 0x7F; 
.................... #endif 
....................  
.................... #if defined(__PCD__)  // Takes care of IEEE format for PCD 
....................    data1 = n+0x7F; 
....................    if(bit_test(data1,0)) 
....................    bit_set(*(((unsigned int8 *)(&res)+2)),7); 
....................    rotate_right(&data1,1); 
....................    bit_clear(data1,7); 
....................    *(((unsigned int8 *)(&res)+3)) = data1; 
.................... #endif 
....................  
....................    y = y/LN2 - (float32)n; 
....................  
....................    r = pe[0]*y + pe[1]; 
....................    r = r*y + pe[2]; 
....................    r = r*y + pe[3]; 
....................    r = r*y + pe[4]; 
....................    r = r*y + pe[5]; 
....................  
....................    res = res*(1.0 + y*r); 
....................  
....................    if (s) 
....................       res = 1.0/res; 
....................    return(res); 
.................... } 
....................  
....................  
.................... //Overloaded function for exp() for PCD 
.................... // Overloaded function exp() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 exp(float48 x) 
.................... { 
....................    float48 y, res, r; 
....................    int8 data1; 
....................    signed int8 n; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x > 88.722838) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    n = (signed int16)(x/LN2); 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       n = -n; 
....................       y = -y; 
....................    } 
....................  
....................    res = 0.0; 
....................  
....................    data1 = n+0x7F; 
....................    if(bit_test(data1,0)) 
....................    bit_set(*(((unsigned int8 *)(&res)+4)),7); 
....................    rotate_right(&data1,1); 
....................    bit_clear(data1,7); 
....................    *(((unsigned int8 *)(&res)+5)) = data1; 
....................  
....................    y = y/LN2 - (float48)n; 
....................  
....................    r = pe[0]*y + pe[1]; 
....................    r = r*y + pe[2]; 
....................    r = r*y + pe[3]; 
....................    r = r*y + pe[4]; 
....................    r = r*y + pe[5]; 
....................  
....................    res = res*(1.0 + y*r); 
....................  
....................    if (s) 
....................       res = 1.0/res; 
....................    return(res); 
.................... } 
....................  
.................... // Overloaded function exp() for data type - Float64 
.................... float64 exp(float64 x) 
.................... { 
....................    float64 y, res, r; 
....................    unsigned int16 data1, data2; 
....................    unsigned int16 *p; 
....................    signed int16 n; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x > 709.7827128) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    n = (signed int16)(x/LN2); 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       n = -n; 
....................       y = -y; 
....................    } 
....................  
....................    res = 0.0; 
....................  
.................... #if !defined(__PCD__) 
....................    *((unsigned int16 *)(&res)) = n + 0x7F; 
.................... #endif 
....................    p= (((unsigned int16 *)(&res))+3); 
....................    data1 = *p; 
....................    data2 = *p;    
....................    data1 = n + 0x3FF; 
....................    data1 = data1 <<4; 
....................    if(bit_test(data2,15)) 
....................    bit_set(data1,15); 
....................    data2 = data2 & 0x000F; 
....................    data1 ^= data2; 
....................  
....................    *(((unsigned int16 *)(&res)+3)) = data1; 
....................  
....................  
....................    y = y/LN2 - (float64)n; 
....................  
....................    r = pe[0]*y + pe[1]; 
....................    r = r*y + pe[2]; 
....................    r = r*y + pe[3]; 
....................    r = r*y + pe[4]; 
....................    r = r*y + pe[5]; 
....................  
....................    res = res*(1.0 + y*r); 
....................  
....................    if (s) 
....................       res = 1.0/res; 
....................    return(res); 
.................... } 
....................  
.................... #ENDIF 
....................  
....................  
.................... /************************************************************/ 
....................  
.................... float32 const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 
.................... float32 const ql[4] = {1.0000000,  -8.1354259, 16.780517, -9.9300943}; 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float log(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the the natural log of x 
.................... // Date : N/A 
.................... // 
.................... float32 log(float32 x) 
.................... { 
....................    float32 y, res, r, y2; 
....................    #if defined(__PCD__) 
....................    unsigned int8  data1,data2; 
....................    #endif 
....................    signed int8 n; 
....................    #ifdef _ERRNO 
....................    if(x <0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    if(x ==0) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    y = x; 
....................  
....................    if (y != 1.0) 
....................    { 
.................... #if !defined(__PCD__) 
....................     *((unsigned int8 *)(&y)) = 0x7E;  
.................... #endif 
....................  
.................... #if defined(__PCD__) // Takes care of IEEE format 
....................    data2 = *(((unsigned int8 *)(&y))+3); 
....................    *(((unsigned int8 *)(&y))+3) = 0x3F; 
....................    data1 = *(((unsigned int8 *)(&y))+2); 
....................    bit_clear(data1,7); 
....................    *(((unsigned int8 *)(&y))+2) = data1; 
....................    if(bit_test(data2,7)) 
....................    bit_set(*(((unsigned int8 *)(&y))+3),7); 
.................... #endif 
....................  
....................       y = (y - 1.0)/(y + 1.0); 
....................  
....................       y2=y*y; 
....................  
....................       res = pl[0]*y2 + pl[1]; 
....................       res = res*y2 + pl[2]; 
....................       res = res*y2 + pl[3]; 
....................  
....................       r = ql[0]*y2 + ql[1]; 
....................       r = r*y2 + ql[2]; 
....................       r = r*y2 + ql[3]; 
....................  
....................       res = y*res/r; 
.................... #if !defined(__PCD__) 
....................       n = *((unsigned int8 *)(&x)) - 0x7E; 
.................... #endif 
.................... #if defined(__PCD__)  
....................     data1 = *(((unsigned int8 *)(&x)+3)); 
....................     rotate_left(&data1,1); 
....................     data2 = *(((unsigned int8 *)(&x)+2)); 
....................     if(bit_test (data2,7)) 
....................       bit_set(data1,0); 
....................     n = data1 - 0x7E; 
.................... #endif 
....................  
....................       if (n<0)  
....................          r = -(float32)-n; 
....................       else 
....................          r = (float32)n; 
....................  
....................       res += r*LN2; 
....................    } 
....................  
....................    else 
....................       res = 0.0; 
....................  
....................    return(res); 
.................... } 
....................  
.................... //Overloaded function for log() for PCD 
.................... // Overloaded function log() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 log(float48 x) 
.................... { 
....................    float48 y, res, r, y2; 
....................    unsigned int8  data1,data2; 
....................    signed int8 n; 
....................    #ifdef _ERRNO 
....................    if(x <0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    if(x ==0) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    y = x; 
....................  
....................    if (y != 1.0) 
....................    { 
....................     
....................    #if !defined(__PCD__) 
....................        *((unsigned int8 *)(&y)) = 0x7E;  
....................    #endif 
....................       data2 = *(((unsigned int8 *)(&y))+5); 
....................       *(((unsigned int8 *)(&y))+5) = 0x3F; 
....................       data1 = *(((unsigned int8 *)(&y))+4); 
....................       bit_clear(data1,7); 
....................       *(((unsigned int8 *)(&y))+4) = data1; 
....................    
....................       if(bit_test(data2,7)) 
....................          bit_set(*(((unsigned int8 *)(&y))+4),7); 
....................       y = (y - 1.0)/(y + 1.0); 
....................  
....................       y2=y*y; 
....................  
....................       res = pl[0]*y2 + pl[1]; 
....................       res = res*y2 + pl[2]; 
....................       res = res*y2 + pl[3]; 
....................  
....................       r = ql[0]*y2 + ql[1]; 
....................       r = r*y2 + ql[2]; 
....................       r = r*y2 + ql[3]; 
....................  
....................       res = y*res/r; 
....................  
....................     data1 = *(((unsigned int8 *)(&x)+5)); 
....................     rotate_left(&data1,1); 
....................     data2 = *(((unsigned int8 *)(&x)+4)); 
....................     if(bit_test (data2,7)) 
....................        bit_set(data1,0); 
....................       
....................     n = data1 - 0x7E; 
....................  
....................       if (n<0) 
....................          r = -(float48)-n; 
....................       else 
....................          r = (float48)n; 
....................  
....................       res += r*LN2; 
....................    } 
....................  
....................    else 
....................       res = 0.0; 
....................  
....................    return(res); 
.................... } 
....................  
.................... // Overloaded function log() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float32 const pl_64[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 
.................... float32 const ql_64[4] = {1.0000000,  -8.1354259, 16.780517, -9.9300943}; 
.................... #endif 
.................... float64 log(float64 x) 
.................... { 
....................    float64 y, res, r, y2; 
....................    unsigned int16  data1,data2; 
....................    unsigned int16 *p; 
....................    signed int16 n; 
....................    #ifdef _ERRNO 
....................    if(x <0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    if(x ==0) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    y = x; 
....................  
....................    if (y != 1.0) 
....................    { 
....................    #if !defined(__PCD__) 
....................        *((unsigned int8 *)(&y)) = 0x7E;  
....................    #endif 
....................       p= (((unsigned int16 *)(&y))+3); 
....................       data1 = *p; 
....................       data2 = *p; 
....................       data1 = 0x3FE; 
....................       data1 = data1 <<4; 
....................       if(bit_test (data2,15)) 
....................       bit_set(data1,15); 
....................       data2 = data2 & 0x000F; 
....................       data1 ^=data2; 
....................  
....................       *p = data1; 
....................  
....................       y = (y - 1.0)/(y + 1.0); 
....................  
....................       y2=y*y; 
....................  
....................       res = pl_64[0]*y2 + pl_64[1]; 
....................       res = res*y2 + pl_64[2]; 
....................       res = res*y2 + pl_64[3]; 
....................  
....................       r = ql_64[0]*y2 + ql_64[1]; 
....................       r = r*y2 + ql_64[2]; 
....................       r = r*y2 + ql_64[3]; 
....................  
....................       res = y*res/r; 
....................   
....................       p= (((unsigned int16 *)(&x))+3); 
....................       data1 = *p; 
....................       bit_clear(data1,15); 
....................       data1 = data1 >>4;     
....................       n = data1 - 0x3FE; 
....................  
....................  
....................       if (n<0) 
....................          r = -(float64)-n; 
....................       else 
....................          r = (float64)n; 
....................  
....................       res += r*LN2; 
....................    } 
....................  
....................    else 
....................       res = 0.0; 
....................  
....................    return(res); 
.................... } 
.................... #endif 
....................  
....................  
.................... #define LN10 2.3025850929940456 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float log10(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the the log base 10 of x 
.................... // Date : N/A 
.................... // 
.................... float32 log10(float32 x) 
.................... { 
....................    float32 r; 
....................  
....................    r = log(x); 
....................    r = r/LN10; 
....................    return(r); 
.................... } 
....................  
.................... //Overloaded functions for log10() for PCD 
.................... // Overloaded function log10() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 log10(float48 x) 
.................... { 
....................    float48 r; 
....................  
....................    r = log(x); 
....................    r = r/LN10; 
....................    return(r); 
.................... } 
....................  
.................... // Overloaded function log10() for data type - Float64 
.................... float64 log10(float64 x) 
.................... { 
....................    float64 r; 
....................  
....................    r = log(x); 
....................    r = r/LN10; 
....................    return(r); 
.................... } 
.................... #endif 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float modf(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description :breaks the argument value int integral and fractional parts, 
.................... // ach of which have the same sign as the argument.  It stores the integral part 
.................... // as a float in the object pointed to by the iptr 
.................... // Returns : returns the signed fractional part of value. 
.................... // Date : N/A 
.................... // 
....................  
.................... float32 modf(float32 value,float32 *iptr) 
.................... { 
....................    *iptr=(value < 0.0)? ceil(value): floor(value); 
....................    return(value - *iptr); 
.................... } 
.................... //Overloaded functions for modf() for PCD 
.................... // Overloaded function modf() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 modf(float48 value,float48 *iptr) 
.................... { 
....................    *iptr=(value < 0.0)? ceil(value): floor(value); 
....................    return(value - *iptr); 
.................... } 
.................... // Overloaded function modf() for data type - Float64 
.................... float64 modf(float64 value,float64 *iptr) 
.................... { 
....................    *iptr=(value < 0.0)? ceil(value): floor(value); 
....................    return(value - *iptr); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float pwr(float x,float y) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the value (x^y) 
.................... // Date : N/A 
.................... // Note : 0 is returned when the function will generate an imaginary number 
.................... // 
.................... float32 pwr(float32 x,float32 y) 
.................... { 
....................    if(0 > x && fmod(y, 1) == 0) { 
....................       if(fmod(y, 2) == 0) { 
....................          return (exp(log(-x) * y)); 
....................       } else { 
....................          return (-exp(log(-x) * y)); 
....................       } 
....................    } else if(0 > x && fmod(y, 1) != 0) { 
....................       return 0; 
....................    } else { 
....................       if(x != 0 || 0 >= y) { 
....................          return (exp(log(x) * y)); 
....................       } 
....................    } 
.................... } 
.................... //Overloaded functions for pwr() for PCD 
.................... // Overloaded function pwr() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 pwr(float48 x,float48 y) 
.................... { 
....................    if(0 > x && fmod(y, 1) == 0) { 
....................       if(fmod(y, 2) == 0) { 
....................          return (exp(log(-x) * y)); 
....................       } else { 
....................          return (-exp(log(-x) * y)); 
....................       } 
....................    } else if(0 > x && fmod(y, 1) != 0) { 
....................       return 0; 
....................    } else { 
....................       if(x != 0 || 0 >= y) { 
....................          return (exp(log(x) * y)); 
....................       } 
....................    } 
.................... } 
.................... // Overloaded function pwr() for data type - Float64 
.................... float64 pwr(float64 x,float64 y) 
.................... { 
....................    if(0 > x && fmod(y, 1) == 0) { 
....................       if(fmod(y, 2) == 0) { 
....................          return (exp(log(-x) * y)); 
....................       } else { 
....................          return (-exp(log(-x) * y)); 
....................       } 
....................    } else if(0 > x && fmod(y, 1) != 0) { 
....................       return 0; 
....................    } else { 
....................       if(x != 0 || 0 >= y) { 
....................          return (exp(log(x) * y)); 
....................       } 
....................    } 
.................... } 
.................... #endif 
....................  
.................... //////////////////// Power functions //////////////////// 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float pow(float x,float y) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the value (x^y) 
.................... // Date : N/A 
.................... // Note : 0 is returned when the function will generate an imaginary number 
.................... // 
.................... float32 pow(float32 x,float32 y) 
.................... { 
....................    if(0 > x && fmod(y, 1) == 0) { 
....................       if(fmod(y, 2) == 0) { 
....................          return (exp(log(-x) * y)); 
....................       } else { 
....................          return (-exp(log(-x) * y)); 
....................       } 
....................    } else if(0 > x && fmod(y, 1) != 0) { 
....................       return 0; 
....................    } else { 
....................       if(x != 0 || 0 >= y) { 
....................          return (exp(log(x) * y)); 
....................       } 
....................    } 
.................... } 
.................... //Overloaded functions for pow() for PCD 
.................... // Overloaded function for pow() data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 pow(float48 x,float48 y) 
.................... { 
....................    if(0 > x && fmod(y, 1) == 0) { 
....................       if(fmod(y, 2) == 0) { 
....................          return (exp(log(-x) * y)); 
....................       } else { 
....................          return (-exp(log(-x) * y)); 
....................       } 
....................    } else if(0 > x && fmod(y, 1) != 0) { 
....................       return 0; 
....................    } else { 
....................       if(x != 0 || 0 >= y) { 
....................          return (exp(log(x) * y)); 
....................       } 
....................    } 
.................... } 
....................  
.................... // Overloaded function pow() for data type - Float64 
.................... float64 pow(float64 x,float64 y) 
.................... { 
....................    if(0 > x && fmod(y, 1) == 0) { 
....................       if(fmod(y, 2) == 0) { 
....................          return (exp(log(-x) * y)); 
....................       } else { 
....................          return (-exp(log(-x) * y)); 
....................       } 
....................    } else if(0 > x && fmod(y, 1) != 0) { 
....................       return 0; 
....................    } else { 
....................       if(x != 0 || 0 >= y) { 
....................          return (exp(log(x) * y)); 
....................       } 
....................    } 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float sqrt(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the square root of x 
.................... // Date : N/A 
.................... // 
.................... float32 sqrt(float32 x) 
.................... { 
....................    float32 y, res; 
....................    #if defined(__PCD__) 
....................    unsigned int16 data1,data2; 
....................    #endif 
....................    BYTE *p; 
....................  
....................    #ifdef _ERRNO 
....................    if(x < 0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................  
....................    if( x<=0.0) 
....................       return(0.0); 
....................  
....................    y=x; 
....................     
....................    #if !defined(__PCD__) 
....................     p=&y; 
....................    (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); 
....................    #endif 
....................     
....................    #if defined(__PCD__) 
....................     p = (((unsigned int8 *)(&y))+3); 
....................     data1 = *(((unsigned int8 *)(&y))+3); 
....................     data2 = *(((unsigned int8 *)(&y))+2); 
....................     rotate_left(&data1,1);     
....................     if(bit_test(data2,7))     
....................     bit_set(data1,0);     
....................     data1 = ((data1+127) >>1); 
....................     bit_clear(data2,7); 
....................     if(bit_test(data1,0)) 
....................     bit_set(data2,7); 
....................     data1 = data1 >>1; 
....................     *(((unsigned int8 *)(&y))+3) = data1; 
....................     *(((unsigned int8 *)(&y))+2) = data2; 
....................      
....................   #endif 
....................  
....................    do { 
....................       res=y; 
....................       y+=(x/y); 
....................  
....................       #if !defined(__PCD__) 
....................      (*p)--; 
....................    #endif 
....................     
....................    #if defined(__PCD__) 
....................     data1 = *(((unsigned int8 *)(&y))+3); 
....................     data2 = *(((unsigned int8 *)(&y))+2); 
....................     rotate_left(&data1,1); 
....................     if(bit_test(data2,7)) 
....................     bit_set(data1,0);     
....................     data1--; 
....................     bit_clear(data2,7); 
....................     if(bit_test(data1,0)) 
....................     bit_set(data2,7); 
....................     data1 = data1 >>1; 
....................     *(((unsigned int8 *)(&y))+3) = data1; 
....................     *(((unsigned int8 *)(&y))+2) = data2; 
....................      
....................   #endif 
....................    } while(res != y); 
....................  
....................    return(res); 
.................... } 
.................... //Overloaded functions for sqrt() for PCD 
.................... // Overloaded function sqrt() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 sqrt(float48 x) 
.................... { 
....................    float48 y, res; 
....................    unsigned int16 data1,data2; 
....................    BYTE *p; 
....................  
....................    #ifdef _ERRNO 
....................    if(x < 0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................  
....................    if( x<=0.0) 
....................       return(0.0); 
....................  
....................    y=x; 
....................     
....................    #if !defined(__PCD__) 
....................     p=&y; 
....................    (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); 
....................    #endif 
....................     
....................    #if defined(__PCD__) 
....................     p = (((unsigned int8 *)(&y))+5); 
....................     data1 = *(((unsigned int8 *)(&y))+5); 
....................     data2 = *(((unsigned int8 *)(&y))+4); 
....................     rotate_left(&data1,1); 
....................     if(bit_test(data2,7)) 
....................     bit_set(data1,0);     
....................     data1 = ((data1+127) >>1); 
....................     bit_clear(data2,7); 
....................     if(bit_test(data1,0)) 
....................     bit_set(data2,7); 
....................     data1 = data1 >>1; 
....................     *(((unsigned int8 *)(&y))+5) = data1; 
....................     *(((unsigned int8 *)(&y))+4) = data2; 
....................      
....................   #endif 
....................  
....................    do { 
....................       res=y; 
....................       y+=(x/y); 
....................  
....................       #if !defined(__PCD__) 
....................      (*p)--; 
....................    #endif 
....................     
....................     data1 = *(((unsigned int8 *)(&y))+5); 
....................     data2 = *(((unsigned int8 *)(&y))+4); 
....................     rotate_left(&data1,1); 
....................     if(bit_test(data2,7)) 
....................     bit_set(data1,0);     
....................     data1--; 
....................     bit_clear(data2,7); 
....................     if(bit_test(data1,0)) 
....................     bit_set(data2,7); 
....................     data1 = data1 >>1; 
....................     *(((unsigned int8 *)(&y))+5) = data1; 
....................     *(((unsigned int8 *)(&y))+4) = data2; 
....................      
....................    } while(res != y); 
....................  
....................    return(res); 
.................... } 
....................  
.................... // Overloaded function sqrt() for data type - Float64 
.................... float64 sqrt(float64 x) 
.................... { 
....................    float64 y, res; 
....................    unsigned int16 *p; 
....................    unsigned int16 temp1,temp2; 
....................  
....................    #ifdef _ERRNO 
....................    if(x < 0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................  
....................    if( x<=0.0) 
....................       return(0.0); 
....................  
....................    y=x; 
....................    p= (((unsigned int16 *)(&y))+3); 
....................    temp1 = *p; 
....................    temp2 = *p; 
....................    bit_clear(temp1,15); 
....................    temp1 = (temp1>>4)+1023; 
....................    temp1 = temp1 >> 1; 
....................    temp1 = (temp1<<4) & 0xFFF0; 
....................    if(bit_test(temp2,15)) 
....................    bit_set(temp1,15); 
....................    temp2 = temp2 & 0x000F; 
....................    temp1 ^= temp2; 
....................     
....................    (*p) = temp1; 
....................     
....................    do { 
....................       res=y; 
....................       y+=(x/y); 
....................      temp1 = *p; 
....................      temp2 = *p; 
....................      bit_clear(temp1,15); 
....................      temp1 = (temp1>>4); 
....................      temp1--; 
....................      temp1 = (temp1<<4) & 0xFFF0; 
....................      if(bit_test(temp2,15)) 
....................      bit_set(temp1,15); 
....................      temp2 = temp2 & 0x000F; 
....................      temp1 ^= temp2; 
....................      (*p) = temp1; 
....................  
....................    } while(res != y); 
....................  
....................    return(res); 
.................... } 
.................... #endif 
....................  
.................... ////////////////////////////// Trig Functions ////////////////////////////// 
.................... #ifdef PI_DIV_BY_TWO 
.................... #undef PI_DIV_BY_TWO 
.................... #endif 
.................... #define PI_DIV_BY_TWO   1.5707963267948966 
.................... #ifdef TWOBYPI 
.................... #undef TWOBYPI 
.................... #define TWOBYPI          0.6366197723675813 
.................... #endif 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float cos(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the cosine value of the angle x, which is in radian 
.................... // Date : 9/20/2001 
.................... // 
.................... float32 cos(float32 x) 
.................... { 
....................    float32 y, t, t2 = 1.0; 
....................    unsigned int8 quad, i; 
....................    float32 frac; 
....................    float32 p[6] = {                    //by the series definition for cosine 
....................       -0.5,                            // sum ( ( (-1)^n * x^2n )/(2n)! ) 
....................        0.04166666666667, 
....................       -0.00138888888889, 
....................        0.00002480158730, 
....................       -0.00000027557319, 
....................        0.00000000208767, 
....................       //-0.00000000001147, 
....................       // 0.00000000000005 
....................    }; 
....................  
....................    if (x < 0) x = -x;                  // absolute value of input 
....................  
....................    quad = (unsigned int8)(x / PI_DIV_BY_TWO);    // quadrant 
....................    frac = (x / PI_DIV_BY_TWO) - quad;  // fractional part of input 
....................    quad = quad % 4;                    // quadrant (0 to 3) 
....................  
....................    if (quad == 0 || quad == 2) 
....................       t = frac * PI_DIV_BY_TWO; 
....................    else if (quad == 1) 
....................       t = (1-frac) * PI_DIV_BY_TWO; 
....................    else // should be 3 
....................       t = (frac-1) * PI_DIV_BY_TWO; 
....................  
....................    y = 1.0; 
....................    t = t * t; 
....................    for (i = 0; i <= 5; i++) 
....................    { 
....................       t2 = t2 * t; 
....................       y = y + p[i] * t2; 
....................    } 
....................  
....................    if (quad == 2 || quad == 1) 
....................       y = -y;  // correct sign 
....................  
....................    return (y); 
.................... } 
....................  
....................  
.................... //Overloaded functions for cos() for PCD 
.................... // Overloaded function cos() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 cos(float48 x) 
.................... { 
....................    float48 y, t, t2 = 1.0; 
....................    unsigned int8 quad, i; 
....................    float48 frac; 
....................    float48 p[6] = {                    //by the series definition for cosine 
....................       -0.5,                            // sum ( ( (-1)^n * x^2n )/(2n)! ) 
....................        0.04166666666667, 
....................       -0.00138888888889, 
....................        0.00002480158730, 
....................       -0.00000027557319, 
....................        0.00000000208767, 
....................       //-0.00000000001147, 
....................       // 0.00000000000005 
....................    }; 
....................  
....................    if (x < 0) x = -x;                  // absolute value of input 
....................  
....................    quad = (unsigned int8)(x / PI_DIV_BY_TWO);    // quadrant 
....................    frac = (x / PI_DIV_BY_TWO) - quad;  // fractional part of input 
....................    quad = quad % 4;                    // quadrant (0 to 3) 
....................  
....................    if (quad == 0 || quad == 2) 
....................       t = frac * PI_DIV_BY_TWO; 
....................    else if (quad == 1) 
....................       t = (1-frac) * PI_DIV_BY_TWO; 
....................    else // should be 3 
....................       t = (frac-1) * PI_DIV_BY_TWO; 
....................  
....................    y = 0.999999999781; 
....................    t = t * t; 
....................    for (i = 0; i <= 5; i++) 
....................    { 
....................       t2 = t2 * t; 
....................       y = y + p[i] * t2; 
....................    } 
....................  
....................    if (quad == 2 || quad == 1) 
....................       y = -y;  // correct sign 
....................  
....................    return (y); 
.................... } 
....................  
.................... // Overloaded function cos() for data type - Float48 
.................... float64 cos(float64 x) 
.................... { 
....................    float64 y, t, t2 = 1.0; 
....................    unsigned int8 quad, i; 
....................    float64 frac; 
....................    float64 p[6] = {                    //by the series definition for cosine 
....................       -0.5,                            // sum ( ( (-1)^n * x^2n )/(2n)! ) 
....................        0.04166666666667, 
....................       -0.00138888888889, 
....................        0.00002480158730, 
....................       -0.00000027557319, 
....................        0.00000000208767, 
....................       //-0.00000000001147, 
....................       // 0.00000000000005 
....................    }; 
....................  
....................    if (x < 0) x = -x;                  // absolute value of input 
....................  
....................    quad = (unsigned int8)(x / PI_DIV_BY_TWO);    // quadrant 
....................    frac = (x / PI_DIV_BY_TWO) - quad;  // fractional part of input 
....................    quad = quad % 4;                    // quadrant (0 to 3) 
....................  
....................    if (quad == 0 || quad == 2) 
....................       t = frac * PI_DIV_BY_TWO; 
....................    else if (quad == 1) 
....................       t = (1-frac) * PI_DIV_BY_TWO; 
....................    else // should be 3 
....................       t = (frac-1) * PI_DIV_BY_TWO; 
....................  
....................    y = 0.999999999781; 
....................    t = t * t; 
....................    for (i = 0; i <= 5; i++) 
....................    { 
....................       t2 = t2 * t; 
....................       y = y + p[i] * t2; 
....................    } 
....................  
....................    if (quad == 2 || quad == 1) 
....................       y = -y;  // correct sign 
....................  
....................    return (y); 
.................... } 
....................  
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float sin(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the sine value of the angle x, which is in radian 
.................... // Date : 9/20/2001 
.................... // 
.................... float32 sin(float32 x) 
.................... { 
....................    return cos(x - PI_DIV_BY_TWO); 
.................... } 
....................  
.................... //Overloaded functions for sin() for PCD 
.................... // Overloaded function sin() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 sin(float48 x) 
.................... { 
....................    return cos(x - PI_DIV_BY_TWO); 
.................... } 
....................  
.................... // Overloaded function sin() for data type - Float48 
.................... float64 sin(float64 x) 
.................... { 
....................    return cos(x - PI_DIV_BY_TWO); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float tan(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the tangent value of the angle x, which is in radian 
.................... // Date : 9/20/2001 
.................... // 
.................... float32 tan(float32 x) 
.................... { 
....................    float32 c, s; 
....................  
....................    c = cos(x); 
....................    if (c == 0.0) 
....................       return (1.0e+36); 
....................  
....................    s = sin(x); 
....................    return(s/c); 
.................... } 
.................... //Overloaded functions for tan() for PCD 
.................... // Overloaded function tan() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 tan(float48 x) 
.................... { 
....................    float48 c, s; 
....................  
....................    c = cos(x); 
....................    if (c == 0.0) 
....................       return (1.0e+36); 
....................  
....................    s = sin(x); 
....................    return(s/c); 
.................... } 
....................  
.................... // Overloaded function tan() for data type - Float48 
.................... float64 tan(float64 x) 
.................... { 
....................    float64 c, s; 
....................  
....................    c = cos(x); 
....................    if (c == 0.0) 
....................       return (1.0e+36); 
....................  
....................    s = sin(x); 
....................    return(s/c); 
.................... } 
.................... #endif 
....................  
.................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290}; 
.................... float32 const qas[3] = {1.0000000,  -5.5484666, 5.6036290}; 
....................  
.................... float32 ASIN_COS(float32 x, unsigned int8 n) 
.................... { 
....................    float32 y, res, r, y2; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x <-1 || x > 1) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 0.5) 
....................    { 
....................       y = sqrt((1.0 - y)/2.0); 
....................       n += 2; 
....................    } 
....................  
....................    y2=y*y; 
....................  
....................    res = pas[0]*y2 + pas[1]; 
....................    res = res*y2 + pas[2]; 
....................  
....................    r = qas[0]*y2 + qas[1]; 
....................    r = r*y2 + qas[2]; 
....................  
....................    res = y*res/r; 
....................  
....................    if (n & 2)     // |x| > 0.5 
....................       res = PI_DIV_BY_TWO - 2.0*res; 
....................    if (s) 
....................       res = -res; 
....................    if (n & 1)           // take arccos 
....................       res = PI_DIV_BY_TWO - res; 
....................  
....................    return(res); 
.................... } 
....................  
.................... //Overloaded functions for ASIN_COS() for PCD 
.................... // Overloaded function ASIN_COS() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 ASIN_COS(float48 x, unsigned int8 n) 
.................... { 
....................    float48 y, res, r, y2; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x <-1 || x > 1) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 0.5) 
....................    { 
....................       y = sqrt((1.0 - y)/2.0); 
....................       n += 2; 
....................    } 
....................  
....................    y2=y*y; 
....................  
....................    res = pas[0]*y2 + pas[1]; 
....................    res = res*y2 + pas[2]; 
....................  
....................    r = qas[0]*y2 + qas[1]; 
....................    r = r*y2 + qas[2]; 
....................  
....................    res = y*res/r; 
....................  
....................    if (n & 2)     // |x| > 0.5 
....................       res = PI_DIV_BY_TWO - 2.0*res; 
....................    if (s) 
....................       res = -res; 
....................    if (n & 1)           // take arccos 
....................       res = PI_DIV_BY_TWO - res; 
....................  
....................    return(res); 
.................... } 
....................  
.................... // Overloaded function ASIN_COS() for data type - Float64 
.................... float64 ASIN_COS(float64 x, unsigned int8 n) 
.................... { 
....................    float64 y, res, r, y2; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x <-1 || x > 1) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 0.5) 
....................    { 
....................       y = sqrt((1.0 - y)/2.0); 
....................       n += 2; 
....................    } 
....................  
....................    y2=y*y; 
....................  
....................    res = pas[0]*y2 + pas[1]; 
....................    res = res*y2 + pas[2]; 
....................  
....................    r = qas[0]*y2 + qas[1]; 
....................    r = r*y2 + qas[2]; 
....................  
....................    res = y*res/r; 
....................  
....................    if (n & 2)     // |x| > 0.5 
....................       res = PI_DIV_BY_TWO - 2.0*res; 
....................    if (s) 
....................       res = -res; 
....................    if (n & 1)           // take arccos 
....................       res = PI_DIV_BY_TWO - res; 
....................  
....................    return(res); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float asin(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the arcsine value of the value x. 
.................... // Date : N/A 
.................... // 
.................... float32 asin(float32 x) 
.................... { 
....................    float32 r; 
....................  
....................    r = ASIN_COS(x, 0); 
....................    return(r); 
.................... } 
.................... //Overloaded functions for asin() for PCD 
.................... // Overloaded function asin() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 asin(float48 x) 
.................... { 
....................    float48 r; 
....................  
....................    r = ASIN_COS(x, 0); 
....................    return(r); 
.................... } 
....................  
.................... // Overloaded function asin() for data type - Float64 
.................... float64 asin(float64 x) 
.................... { 
....................    float64 r; 
....................  
....................    r = ASIN_COS(x, 0); 
....................    return(r); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float acos(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the arccosine value of the value x. 
.................... // Date : N/A 
.................... // 
.................... float32 acos(float32 x) 
.................... { 
....................    float32 r; 
....................  
....................    r = ASIN_COS(x, 1); 
....................    return(r); 
.................... } 
.................... //Overloaded functions for acos() for PCD 
.................... // Overloaded function acos() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 acos(float48 x) 
.................... { 
....................    float48 r; 
....................  
....................    r = ASIN_COS(x, 1); 
....................    return(r); 
.................... } 
....................  
.................... // Overloaded function acos() for data type - Float64 
.................... float64 acos(float64 x) 
.................... { 
....................    float64 r; 
....................  
....................    r = ASIN_COS(x, 1); 
....................    return(r); 
.................... } 
.................... #endif 
....................  
.................... float32 const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; 
.................... float32 const qat[4] = {1.0000000,  11.368190, 28.982246, 19.818457}; 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float atan(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the arctangent value of the value x. 
.................... // Date : N/A 
.................... // 
.................... float32 atan(float32 x) 
.................... { 
....................    float32 y, res, r; 
....................    int1 s, flag; 
....................  
....................    s = 0; 
....................    flag = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 1.0) 
....................    { 
....................       y = 1.0/y; 
....................       flag = 1; 
....................    } 
....................  
....................    res = pat[0]*y*y + pat[1]; 
....................    res = res*y*y + pat[2]; 
....................    res = res*y*y + pat[3]; 
....................  
....................    r = qat[0]*y*y + qat[1]; 
....................    r = r*y*y + qat[2]; 
....................    r = r*y*y + qat[3]; 
....................  
....................    res = y*res/r; 
....................  
....................  
....................    if (flag)                              // for |x| > 1 
....................       res = PI_DIV_BY_TWO - res; 
....................    if (s) 
....................       res = -res; 
....................  
....................    return(res); 
.................... } 
.................... //Overloaded functions for atan() for PCD 
.................... // Overloaded function atan() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 atan(float48 x) 
.................... { 
....................    float48 y, res, r; 
....................    int1 s, flag; 
....................  
....................    s = 0; 
....................    flag = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 1.0) 
....................    { 
....................       y = 1.0/y; 
....................       flag = 1; 
....................    } 
....................  
....................    res = pat[0]*y*y + pat[1]; 
....................    res = res*y*y + pat[2]; 
....................    res = res*y*y + pat[3]; 
....................  
....................    r = qat[0]*y*y + qat[1]; 
....................    r = r*y*y + qat[2]; 
....................    r = r*y*y + qat[3]; 
....................  
....................    res = y*res/r; 
....................  
....................  
....................    if (flag)                              // for |x| > 1 
....................       res = PI_DIV_BY_TWO - res; 
....................    if (s) 
....................       res = -res; 
....................  
....................    return(res); 
.................... } 
....................  
.................... // Overloaded function atan() for data type - Float64 
.................... float64 atan(float64 x) 
.................... { 
....................    float64 y, res, r; 
....................    int1 s, flag; 
....................  
....................    s = 0; 
....................    flag = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 1.0) 
....................    { 
....................       y = 1.0/y; 
....................       flag = 1; 
....................    } 
....................  
....................    res = pat[0]*y*y + pat[1]; 
....................    res = res*y*y + pat[2]; 
....................    res = res*y*y + pat[3]; 
....................  
....................    r = qat[0]*y*y + qat[1]; 
....................    r = r*y*y + qat[2]; 
....................    r = r*y*y + qat[3]; 
....................  
....................    res = y*res/r; 
....................  
....................  
....................    if (flag)                              // for |x| > 1 
....................       res = PI_DIV_BY_TWO - res; 
....................    if (s) 
....................       res = -res; 
....................  
....................    return(res); 
.................... } 
.................... #endif 
....................  
.................... ///////////////////////////////////////////////////////////////////////////// 
.................... //   float atan2(float y, float x) 
.................... ///////////////////////////////////////////////////////////////////////////// 
.................... // Description :computes the principal value of arc tangent of y/x, using the 
.................... // signs of both the arguments to determine the quadrant of the return value 
.................... // Returns : returns the arc tangent of y/x. 
.................... // Date : N/A 
.................... // 
....................  
.................... float32 atan2(float32 y,float32 x) 
.................... { 
....................    float32 z; 
....................    int1 sign; 
....................    unsigned int8 quad; 
....................    sign=0; 
....................    quad=0; //quadrant 
....................    quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 
....................    if(y<0.0) 
....................    { 
....................       sign=1; 
....................       y=-y; 
....................    } 
....................    if(x<0.0) 
....................    { 
....................       x=-x; 
....................    } 
....................    if (x==0.0) 
....................    { 
....................       if(y==0.0) 
....................       { 
....................       #ifdef _ERRNO 
....................       { 
....................          errno=EDOM; 
....................       } 
....................       #endif 
....................       } 
....................       else 
....................       { 
....................          if(sign) 
....................          { 
....................          return (-(PI_DIV_BY_TWO)); 
....................          } 
....................          else 
....................          { 
....................          return (PI_DIV_BY_TWO); 
....................          } 
....................       } 
....................    } 
....................    else 
....................    { 
....................       z=y/x; 
....................       switch(quad) 
....................       { 
....................          case 1: 
....................          { 
....................             return atan(z); 
....................             break; 
....................          } 
....................          case 2: 
....................          { 
.................... //            return (atan(z)+PI_DIV_BY_TWO);  //2L3122 
....................             return (PI-atan(z)); 
....................             break; 
....................          } 
....................          case 3: 
....................          { 
....................             return (atan(z)-PI); 
....................             break; 
....................          } 
....................          case 4: 
....................          { 
....................             return (-atan(z)); 
....................             break; 
....................          } 
....................       } 
....................    } 
.................... } 
....................  
.................... //Overloaded functions for atan2() for PCD 
.................... // Overloaded function atan2() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 atan2(float48 y,float48 x) 
.................... { 
....................    float48 z; 
....................    int1 sign; 
....................    unsigned int8 quad; 
....................    sign=0; 
....................    quad=0; //quadrant 
....................    quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 
....................    if(y<0.0) 
....................    { 
....................       sign=1; 
....................       y=-y; 
....................    } 
....................    if(x<0.0) 
....................    { 
....................       x=-x; 
....................    } 
....................    if (x==0.0) 
....................    { 
....................       if(y==0.0) 
....................       { 
....................       #ifdef _ERRNO 
....................       { 
....................          errno=EDOM; 
....................       } 
....................       #endif 
....................       } 
....................       else 
....................       { 
....................          if(sign) 
....................          { 
....................          return (-(PI_DIV_BY_TWO)); 
....................          } 
....................          else 
....................          { 
....................          return (PI_DIV_BY_TWO); 
....................          } 
....................       } 
....................    } 
....................    else 
....................    { 
....................       z=y/x; 
....................       switch(quad) 
....................       { 
....................          case 1: 
....................          { 
....................             return atan(z); 
....................             break; 
....................          } 
....................          case 2: 
....................          { 
.................... //            return (atan(z)+PI_DIV_BY_TWO);  //2L3122 
....................             return (PI-atan(z)); 
....................             break; 
....................          } 
....................          case 3: 
....................          { 
....................             return (atan(z)-PI); 
....................             break; 
....................          } 
....................          case 4: 
....................          { 
....................             return (-atan(z)); 
....................             break; 
....................          } 
....................       } 
....................    } 
.................... } 
....................  
.................... // Overloaded function atan2() for data type - Float64 
.................... float64 atan2(float64 y,float64 x) 
.................... { 
....................    float64 z; 
....................    int1 sign; 
....................    unsigned int8 quad; 
....................    sign=0; 
....................    quad=0; //quadrant 
....................    quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 
....................    if(y<0.0) 
....................    { 
....................       sign=1; 
....................       y=-y; 
....................    } 
....................    if(x<0.0) 
....................    { 
....................       x=-x; 
....................    } 
....................    if (x==0.0) 
....................    { 
....................       if(y==0.0) 
....................       { 
....................       #ifdef _ERRNO 
....................       { 
....................          errno=EDOM; 
....................       } 
....................       #endif 
....................       } 
....................       else 
....................       { 
....................          if(sign) 
....................          { 
....................          return (-(PI_DIV_BY_TWO)); 
....................          } 
....................          else 
....................          { 
....................          return (PI_DIV_BY_TWO); 
....................          } 
....................       } 
....................    } 
....................    else 
....................    { 
....................       z=y/x; 
....................       switch(quad) 
....................       { 
....................          case 1: 
....................          { 
....................             return atan(z); 
....................             break; 
....................          } 
....................          case 2: 
....................          { 
.................... //            return (atan(z)+PI_DIV_BY_TWO);  //2L3122 
....................             return (PI-atan(z)); 
....................             break; 
....................          } 
....................          case 3: 
....................          { 
....................             return (atan(z)-PI); 
....................             break; 
....................          } 
....................          case 4: 
....................          { 
....................             return (-atan(z)); 
....................             break; 
....................          } 
....................       } 
....................    } 
.................... } 
.................... #endif 
....................  
.................... //////////////////// Hyperbolic functions //////////////////// 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float cosh(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the hyperbolic cosine value of x 
.................... // Returns : returns the hyperbolic cosine value of x 
.................... // Date : N/A 
.................... // 
....................  
.................... float32 cosh(float32 x) 
.................... { 
....................    return ((exp(x)+exp(-x))/2); 
.................... } 
.................... //Overloaded functions for cosh() for PCD 
.................... // Overloaded function cosh() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 cosh(float48 x) 
.................... { 
....................    return ((exp(x)+exp(-x))/2); 
.................... } 
....................  
.................... // Overloaded function cosh() for data type - Float64 
.................... float64 cosh(float64 x) 
.................... { 
....................    return ((exp(x)+exp(-x))/2); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float sinh(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the hyperbolic sine value of x 
.................... // Returns : returns the hyperbolic sine value of x 
.................... // Date : N/A 
.................... // 
....................  
.................... float32 sinh(float32 x) 
.................... { 
....................  
....................    return ((exp(x) - exp(-x))/2); 
.................... } 
.................... //Overloaded functions for sinh() for PCD 
.................... // Overloaded function sinh() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 sinh(float48 x) 
.................... { 
....................  
....................    return ((exp(x) - exp(-x))/2); 
.................... } 
....................  
.................... // Overloaded function sinh() for data type - Float48 
.................... float64 sinh(float64 x) 
.................... { 
....................  
....................    return ((exp(x) - exp(-x))/2); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float tanh(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the hyperbolic tangent value of x 
.................... // Returns : returns the hyperbolic tangent value of x 
.................... // Date : N/A 
.................... // 
....................  
.................... float32 tanh(float32 x) 
.................... { 
....................    return(sinh(x)/cosh(x)); 
.................... } 
.................... //Overloaded functions for tanh() for PCD 
.................... // Overloaded function tanh() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 tanh(float48 x) 
.................... { 
....................    return(sinh(x)/cosh(x)); 
.................... } 
....................  
.................... // Overloaded function tanh() for data type - Float64 
.................... float64 tanh(float64 x) 
.................... { 
....................    return(sinh(x)/cosh(x)); 
.................... } 
.................... #endif 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //   float frexp(float x, signed int *exp) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : breaks a floating point number into a normalized fraction and an integral 
.................... // power of 2. It stores the integer in the signed int object pointed to by exp. 
.................... // Returns : returns the value x, such that x is a double with magnitude in the interval 
.................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero, 
.................... // both parts of the result are zero. 
.................... // Date : N/A 
.................... // 
....................  
.................... #define LOG2 .30102999566398119521 
.................... float32 frexp(float32 x, signed int8 *exp) 
.................... { 
....................    float32 res; 
....................    int1 sign = 0; 
....................    if(x == 0.0) 
....................    { 
....................       *exp=0; 
....................       return (0.0); 
....................    } 
....................    if(x < 0.0) 
....................    { 
....................      x=-x; 
....................      sign=1; 
....................    } 
....................    if (x > 1.0) 
....................    { 
....................       *exp=(ceil(log10(x)/LOG2)); 
....................       res=x/(pow(2, *exp)); 
....................       if (res == 1) 
....................       { 
....................          *exp=*exp+1; 
....................           res=.5; 
....................       } 
....................    } 
....................    else 
....................    { 
....................       if(x < 0.5) 
....................       { 
....................          *exp=-1; 
....................          res=x*2; 
....................       } 
....................       else 
....................       { 
....................          *exp=0; 
....................           res=x; 
....................       } 
....................    } 
....................    if(sign) 
....................    { 
....................       res=-res; 
....................    } 
....................    return res; 
.................... } 
....................  
.................... //Overloaded functions for frexp() for PCD 
.................... // Overloaded function frexp() for data type - Float48 
.................... #if defined(__PCD__) 
.................... float48 frexp(float48 x, signed int8 *exp) 
.................... { 
....................    float48 res; 
....................    int1 sign = 0; 
....................    if(x == 0.0) 
....................    { 
....................       *exp=0; 
....................       return (0.0); 
....................    } 
....................    if(x < 0.0) 
....................    { 
....................      x=-x; 
....................      sign=1; 
....................    } 
....................    if (x > 1.0) 
....................    { 
....................       *exp=(ceil(log10(x)/LOG2)); 
....................       res=x/(pow(2, *exp)); 
....................       if (res == 1) 
....................       { 
....................          *exp=*exp+1; 
....................           res=.5; 
....................       } 
....................    } 
....................    else 
....................    { 
....................       if(x < 0.5) 
....................       { 
....................          *exp=-1; 
....................          res=x*2; 
....................       } 
....................       else 
....................       { 
....................          *exp=0; 
....................           res=x; 
....................       } 
....................    } 
....................    if(sign) 
....................    { 
....................       res=-res; 
....................    } 
....................    return res; 
.................... } 
....................  
.................... // Overloaded function frexp() for data type - Float64 
.................... float64 frexp(float64 x, signed int8 *exp) 
.................... { 
....................    float64 res; 
....................    int1 sign = 0; 
....................    if(x == 0.0) 
....................    { 
....................       *exp=0; 
....................       return (0.0); 
....................    } 
....................    if(x < 0.0) 
....................    { 
....................      x=-x; 
....................      sign=1; 
....................    } 
....................    if (x > 1.0) 
....................    { 
....................       *exp=(ceil(log10(x)/LOG2)); 
....................       res=x/(pow(2, *exp)); 
....................       if (res == 1) 
....................       { 
....................          *exp=*exp+1; 
....................           res=.5; 
....................       } 
....................    } 
....................    else 
....................    { 
....................       if(x < 0.5) 
....................       { 
....................          *exp=-1; 
....................          res=x*2; 
....................       } 
....................       else 
....................       { 
....................          *exp=0; 
....................           res=x; 
....................       } 
....................    } 
....................    if(sign) 
....................    { 
....................       res=-res; 
....................    } 
....................    return res; 
.................... } 
.................... #endif 
....................  
.................... ////////////////////////////////////////////////////////////////////////////// 
.................... //   float ldexp(float x, signed int *exp) 
.................... ////////////////////////////////////////////////////////////////////////////// 
.................... // Description : multiplies a floating point number by an integral power of 2. 
.................... // Returns : returns the value of x times 2 raised to the power exp. 
.................... // Date : N/A 
.................... // 
....................  
.................... float32 ldexp(float32 value, signed int8 exp) 
.................... { 
....................    return (value * pow(2,exp)); 
.................... } 
.................... //Overloaded functions for ldexp() for PCD 
.................... // Overloaded function ldexp() for data type - Float48 
....................  
.................... #if defined(__PCD__) 
.................... float48 ldexp(float48 value, signed int8 exp) 
.................... { 
....................    return (value * pow(2,exp)); 
.................... } 
.................... // Overloaded function ldexp() for data type - Float64 
.................... float64 ldexp(float64 value, signed int8 exp) 
.................... { 
....................    return (value * pow(2,exp)); 
.................... } 
.................... #endif 
....................  
.................... #endif 
....................  
....................  
.................... void main() 
.................... { 
*
0266:  CLRF   04
0267:  BCF    03.7
0268:  MOVLW  1F
0269:  ANDWF  03,F
026A:  MOVLW  71
026B:  BSF    03.5
026C:  MOVWF  0F
026D:  MOVF   0F,W
026E:  BCF    03.5
026F:  BCF    20.7
0270:  MOVF   20,W
0271:  BSF    03.5
0272:  MOVWF  07
0273:  BCF    03.5
0274:  BSF    07.7
0275:  BSF    03.5
0276:  BSF    03.6
0277:  MOVF   09,W
0278:  ANDLW  C0
0279:  MOVWF  09
027A:  BCF    03.6
027B:  BCF    1F.4
027C:  BCF    1F.5
027D:  MOVLW  00
027E:  BSF    03.6
027F:  MOVWF  08
0280:  BCF    03.5
0281:  CLRF   07
0282:  CLRF   08
0283:  CLRF   09
*
028D:  CLRF   34
028E:  CLRF   33
.................... float last,b,anemo_speed; 
.................... unsigned int16 anemo_round=0; 
.................... unsigned int16 i; 
....................  
.................... //signed int16 X,Y,Z; 
....................   setup_adc_ports(NO_ANALOGS|VSS_VDD); 
028F:  BSF    03.5
0290:  BSF    03.6
0291:  MOVF   09,W
0292:  ANDLW  C0
0293:  MOVWF  09
0294:  BCF    03.6
0295:  BCF    1F.4
0296:  BCF    1F.5
0297:  MOVLW  00
0298:  BSF    03.6
0299:  MOVWF  08
....................   setup_adc(ADC_CLOCK_DIV_2); 
029A:  BCF    03.5
029B:  BCF    03.6
029C:  BCF    1F.6
029D:  BCF    1F.7
029E:  BSF    03.5
029F:  BCF    1F.7
02A0:  BCF    03.5
02A1:  BSF    1F.0
....................   setup_spi(SPI_SS_DISABLED); 
02A2:  BCF    14.5
02A3:  BCF    20.5
02A4:  MOVF   20,W
02A5:  BSF    03.5
02A6:  MOVWF  07
02A7:  BCF    03.5
02A8:  BSF    20.4
02A9:  MOVF   20,W
02AA:  BSF    03.5
02AB:  MOVWF  07
02AC:  BCF    03.5
02AD:  BCF    20.3
02AE:  MOVF   20,W
02AF:  BSF    03.5
02B0:  MOVWF  07
02B1:  MOVLW  01
02B2:  BCF    03.5
02B3:  MOVWF  14
02B4:  MOVLW  00
02B5:  BSF    03.5
02B6:  MOVWF  14
....................   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 
02B7:  MOVF   01,W
02B8:  ANDLW  C7
02B9:  IORLW  08
02BA:  MOVWF  01
....................   setup_timer_1(T1_DISABLED); 
02BB:  BCF    03.5
02BC:  CLRF   10
....................   setup_timer_2(T2_DISABLED,0,1); 
02BD:  MOVLW  00
02BE:  MOVWF  78
02BF:  MOVWF  12
02C0:  MOVLW  00
02C1:  BSF    03.5
02C2:  MOVWF  12
....................   setup_ccp1(CCP_OFF); 
02C3:  BCF    03.5
02C4:  BSF    20.2
02C5:  MOVF   20,W
02C6:  BSF    03.5
02C7:  MOVWF  07
02C8:  BCF    03.5
02C9:  CLRF   17
02CA:  BSF    03.5
02CB:  CLRF   1B
02CC:  CLRF   1C
02CD:  MOVLW  01
02CE:  MOVWF  1D
....................   setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard 
02CF:  BCF    03.5
02D0:  BSF    03.6
02D1:  CLRF   07
02D2:  CLRF   08
02D3:  CLRF   09
....................  
....................   printf("Magnetometr:  \r\n",); 
02D4:  MOVLW  04
02D5:  MOVWF  0D
02D6:  MOVLW  00
02D7:  MOVWF  0F
02D8:  BCF    03.6
02D9:  CALL   043
....................   printf("(c)mlab JACHO 2013:  \r\n",); 
02DA:  MOVLW  0D
02DB:  BSF    03.6
02DC:  MOVWF  0D
02DD:  MOVLW  00
02DE:  MOVWF  0F
02DF:  BCF    03.6
02E0:  CALL   043
....................   printf("X, Y, Z \r\n",); 
02E1:  MOVLW  19
02E2:  BSF    03.6
02E3:  MOVWF  0D
02E4:  MOVLW  00
02E5:  MOVWF  0F
02E6:  BCF    03.6
02E7:  CALL   043
....................  
....................  
.................... // Init the HMC5883L.  Set Mode register for 
.................... // continuous measurements. 
.................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18);      // no average, maximal update range 
02E8:  CLRF   37
02E9:  MOVLW  18
02EA:  MOVWF  38
02EB:  CALL   0D3
.................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0xE0);      // maximal range 
02EC:  MOVLW  01
02ED:  MOVWF  37
02EE:  MOVLW  E0
02EF:  MOVWF  38
02F0:  CALL   0D3
.................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00); 
02F1:  MOVLW  02
02F2:  MOVWF  37
02F3:  CLRF   38
02F4:  CALL   0D3
....................  
.................... // Continuously read and display the x,y,z results. 
.................... // Wait at least 67 ms between reads, re the HMC5883L data sheet. 
....................  
....................  
.................... while(TRUE) 
....................   { 
....................  
....................     hmc5883l_read_data();    
02F5:  GOTO   151
....................     printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z); 
02F6:  MOVLW  00
02F7:  MOVWF  04
02F8:  MOVF   22,W
02F9:  MOVWF  38
02FA:  MOVF   21,W
02FB:  MOVWF  37
02FC:  CALL   1D0
02FD:  MOVLW  20
02FE:  MOVWF  40
02FF:  CALL   01F
0300:  MOVLW  00
0301:  MOVWF  04
0302:  MOVF   24,W
0303:  MOVWF  38
0304:  MOVF   23,W
0305:  MOVWF  37
0306:  CALL   1D0
0307:  MOVLW  20
0308:  MOVWF  40
0309:  CALL   01F
030A:  MOVLW  00
030B:  MOVWF  04
030C:  MOVF   26,W
030D:  MOVWF  38
030E:  MOVF   25,W
030F:  MOVWF  37
0310:  CALL   1D0
0311:  MOVLW  20
0312:  MOVWF  40
0313:  CALL   01F
0314:  MOVLW  0A
0315:  MOVWF  40
0316:  CALL   01F
0317:  MOVLW  0D
0318:  MOVWF  40
0319:  CALL   01F
....................     delay_ms(100); 
031A:  MOVLW  64
031B:  MOVWF  37
031C:  GOTO   250
....................   }     
031D:  GOTO   2F5
....................  
.................... } 
....................  
....................  
....................  
031E:  SLEEP

Configuration Fuses:
   Word  1: 2CF5   INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG
   Word  2: 3FFF   NOWRT BORV40