/Modules/Sensors/IMU01A/SW/PIC16F887/main.lst
0,0 → 1,3085
CCS PCM C Compiler, Version 4.106, 47914 11-5-13 10:50
 
Filename: D:\Honza\MLAB\Modules\Sensors\IMU01A\SW\PIC16F887\main.lst
 
ROM used: 1146 words (14%)
Largest free fragment is 2048
RAM used: 13 (4%) at main() level
26 (7%) worst case
Stack: 3 locations
 
*
0000: MOVLW 03
0001: MOVWF 0A
0002: GOTO 397
0003: NOP
.................... //Akcelerometr IMU01A
.................... //(c) MLAB Jan Chroust 2013
....................
....................
.................... #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)
*
0381: MOVLW 28
0382: MOVWF 04
0383: BCF 03.7
0384: MOVF 00,W
0385: BTFSC 03.2
0386: GOTO 394
0387: MOVLW 02
0388: MOVWF 78
0389: CLRF 77
038A: DECFSZ 77,F
038B: GOTO 38A
038C: DECFSZ 78,F
038D: GOTO 389
038E: MOVLW 97
038F: MOVWF 77
0390: DECFSZ 77,F
0391: GOTO 390
0392: DECFSZ 00,F
0393: GOTO 387
0394: BCF 0A.3
0395: BCF 0A.4
0396: GOTO 476 (RETURN)
....................
....................
....................
.................... #define PIN_SDA PIN_C4
.................... #define PIN_SCL PIN_C3
.................... #use i2c(master, sda=PIN_SDA, scl=PIN_SCL) //I2C
*
0041: MOVLW 08
0042: MOVWF 78
0043: NOP
0044: BCF 07.3
0045: BCF 20.3
0046: MOVF 20,W
0047: BSF 03.5
0048: MOVWF 07
0049: NOP
004A: BCF 03.5
004B: RLF 2F,F
004C: BCF 07.4
004D: BTFSS 03.0
004E: GOTO 055
004F: BSF 20.4
0050: MOVF 20,W
0051: BSF 03.5
0052: MOVWF 07
0053: GOTO 059
0054: BCF 03.5
0055: BCF 20.4
0056: MOVF 20,W
0057: BSF 03.5
0058: MOVWF 07
0059: NOP
005A: BCF 03.5
005B: BSF 20.3
005C: MOVF 20,W
005D: BSF 03.5
005E: MOVWF 07
005F: BCF 03.5
0060: BTFSS 07.3
0061: GOTO 060
0062: DECFSZ 78,F
0063: GOTO 043
0064: NOP
0065: BCF 07.3
0066: BCF 20.3
0067: MOVF 20,W
0068: BSF 03.5
0069: MOVWF 07
006A: NOP
006B: BCF 03.5
006C: BSF 20.4
006D: MOVF 20,W
006E: BSF 03.5
006F: MOVWF 07
0070: NOP
0071: NOP
0072: BCF 03.5
0073: BSF 20.3
0074: MOVF 20,W
0075: BSF 03.5
0076: MOVWF 07
0077: BCF 03.5
0078: BTFSS 07.3
0079: GOTO 078
007A: CLRF 78
007B: NOP
007C: BTFSC 07.4
007D: BSF 78.0
007E: BCF 07.3
007F: BCF 20.3
0080: MOVF 20,W
0081: BSF 03.5
0082: MOVWF 07
0083: BCF 03.5
0084: BCF 07.4
0085: BCF 20.4
0086: MOVF 20,W
0087: BSF 03.5
0088: MOVWF 07
0089: BCF 03.5
008A: RETURN
*
0130: MOVLW 08
0131: MOVWF 30
0132: MOVF 77,W
0133: MOVWF 31
0134: BSF 20.4
0135: MOVF 20,W
0136: BSF 03.5
0137: MOVWF 07
0138: NOP
0139: BCF 03.5
013A: BSF 20.3
013B: MOVF 20,W
013C: BSF 03.5
013D: MOVWF 07
013E: BCF 03.5
013F: BTFSS 07.3
0140: GOTO 13F
0141: BTFSC 07.4
0142: BSF 03.0
0143: BTFSS 07.4
0144: BCF 03.0
0145: RLF 78,F
0146: NOP
0147: BCF 20.3
0148: MOVF 20,W
0149: BSF 03.5
014A: MOVWF 07
014B: BCF 03.5
014C: BCF 07.3
014D: DECFSZ 30,F
014E: GOTO 134
014F: BSF 20.4
0150: MOVF 20,W
0151: BSF 03.5
0152: MOVWF 07
0153: NOP
0154: BCF 03.5
0155: BCF 07.4
0156: MOVF 31,W
0157: BTFSC 03.2
0158: GOTO 15E
0159: BCF 20.4
015A: MOVF 20,W
015B: BSF 03.5
015C: MOVWF 07
015D: BCF 03.5
015E: NOP
015F: BSF 20.3
0160: MOVF 20,W
0161: BSF 03.5
0162: MOVWF 07
0163: BCF 03.5
0164: BTFSS 07.3
0165: GOTO 164
0166: NOP
0167: BCF 07.3
0168: BCF 20.3
0169: MOVF 20,W
016A: BSF 03.5
016B: MOVWF 07
016C: NOP
016D: BCF 03.5
016E: BCF 07.4
016F: BCF 20.4
0170: MOVF 20,W
0171: BSF 03.5
0172: MOVWF 07
0173: BCF 03.5
0174: RETURN
.................... #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD
*
00C6: BCF 20.7
00C7: MOVF 20,W
00C8: BSF 03.5
00C9: MOVWF 07
00CA: BCF 03.5
00CB: BCF 07.7
00CC: MOVLW 08
00CD: MOVWF 78
00CE: GOTO 0CF
00CF: NOP
00D0: BSF 78.7
00D1: GOTO 0E0
00D2: BCF 78.7
00D3: RRF 30,F
00D4: BTFSC 03.0
00D5: BSF 07.7
00D6: BTFSS 03.0
00D7: BCF 07.7
00D8: BSF 78.6
00D9: GOTO 0E0
00DA: BCF 78.6
00DB: DECFSZ 78,F
00DC: GOTO 0D3
00DD: GOTO 0DE
00DE: NOP
00DF: BSF 07.7
00E0: MOVLW 3F
00E1: MOVWF 04
00E2: DECFSZ 04,F
00E3: GOTO 0E2
00E4: NOP
00E5: BTFSC 78.7
00E6: GOTO 0D2
00E7: BTFSC 78.6
00E8: GOTO 0DA
00E9: 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
....................
.................... #include <MMA8451Q.h>
.................... //Adresa obvodu
.................... #define AK_W 0x38 //adresa akcelerometru zápis
.................... #define AK_R 0x39 //adresa akcelerometru ètení
....................
....................
.................... #define AK_XH 0x01 //osa X LSB
.................... #define AK_XL 0x02 //osa X MSB
.................... #define AK_YH 0x03 //osa Y LSB
.................... #define AK_YL 0x04 //osa Y MSB
.................... #define AK_ZH 0x05 //osa Z LSB
.................... #define AK_ZL 0x06 //osa Z MSB
....................
....................
.................... #include "MMA8451Q.c"
.................... void setAK (void) //nastaveni akcelerometru
.................... {
.................... i2c_start();
*
008B: BSF 20.4
008C: MOVF 20,W
008D: BSF 03.5
008E: MOVWF 07
008F: NOP
0090: BCF 03.5
0091: BSF 20.3
0092: MOVF 20,W
0093: BSF 03.5
0094: MOVWF 07
0095: NOP
0096: BCF 03.5
0097: BCF 07.4
0098: BCF 20.4
0099: MOVF 20,W
009A: BSF 03.5
009B: MOVWF 07
009C: NOP
009D: BCF 03.5
009E: BCF 07.3
009F: BCF 20.3
00A0: MOVF 20,W
00A1: BSF 03.5
00A2: MOVWF 07
.................... I2C_Write(AK_W);
00A3: MOVLW 38
00A4: BCF 03.5
00A5: MOVWF 2F
00A6: CALL 041
.................... I2C_write(0x2A);
00A7: MOVLW 2A
00A8: MOVWF 2F
00A9: CALL 041
.................... I2C_write(0x01); //nastavi aktivni stav
00AA: MOVLW 01
00AB: MOVWF 2F
00AC: CALL 041
....................
.................... i2c_stop();
00AD: BCF 20.4
00AE: MOVF 20,W
00AF: BSF 03.5
00B0: MOVWF 07
00B1: NOP
00B2: BCF 03.5
00B3: BSF 20.3
00B4: MOVF 20,W
00B5: BSF 03.5
00B6: MOVWF 07
00B7: BCF 03.5
00B8: BTFSS 07.3
00B9: GOTO 0B8
00BA: NOP
00BB: GOTO 0BC
00BC: NOP
00BD: BSF 20.4
00BE: MOVF 20,W
00BF: BSF 03.5
00C0: MOVWF 07
00C1: NOP
.................... }
00C2: BCF 03.5
00C3: BCF 0A.3
00C4: BCF 0A.4
00C5: GOTO 3FF (RETURN)
....................
....................
.................... int16 akR (int8 H, int8 L) //vycitani hodnot z akcelerometru
.................... {
*
0175: CLRF 2B
0176: CLRF 2C
.................... unsigned int8 XL=0,XH=0;
.................... signed int16 x;
....................
.................... i2c_start();
0177: BSF 20.4
0178: MOVF 20,W
0179: BSF 03.5
017A: MOVWF 07
017B: NOP
017C: BCF 03.5
017D: BSF 20.3
017E: MOVF 20,W
017F: BSF 03.5
0180: MOVWF 07
0181: NOP
0182: BCF 03.5
0183: BCF 07.4
0184: BCF 20.4
0185: MOVF 20,W
0186: BSF 03.5
0187: MOVWF 07
0188: NOP
0189: BCF 03.5
018A: BCF 07.3
018B: BCF 20.3
018C: MOVF 20,W
018D: BSF 03.5
018E: MOVWF 07
.................... I2C_Write(AK_W);
018F: MOVLW 38
0190: BCF 03.5
0191: MOVWF 2F
0192: CALL 041
.................... I2C_write(H);
0193: MOVF 29,W
0194: MOVWF 2F
0195: CALL 041
.................... i2c_start();
0196: BSF 20.4
0197: MOVF 20,W
0198: BSF 03.5
0199: MOVWF 07
019A: NOP
019B: BCF 03.5
019C: BSF 20.3
019D: MOVF 20,W
019E: BSF 03.5
019F: MOVWF 07
01A0: NOP
01A1: BCF 03.5
01A2: BTFSS 07.3
01A3: GOTO 1A2
01A4: BCF 07.4
01A5: BCF 20.4
01A6: MOVF 20,W
01A7: BSF 03.5
01A8: MOVWF 07
01A9: NOP
01AA: BCF 03.5
01AB: BCF 07.3
01AC: BCF 20.3
01AD: MOVF 20,W
01AE: BSF 03.5
01AF: MOVWF 07
.................... I2C_Write(AK_R);
01B0: MOVLW 39
01B1: BCF 03.5
01B2: MOVWF 2F
01B3: CALL 041
.................... XH=i2c_read(0);
01B4: CLRF 77
01B5: CALL 130
01B6: MOVF 78,W
01B7: MOVWF 2C
.................... i2c_stop();
01B8: BCF 20.4
01B9: MOVF 20,W
01BA: BSF 03.5
01BB: MOVWF 07
01BC: NOP
01BD: BCF 03.5
01BE: BSF 20.3
01BF: MOVF 20,W
01C0: BSF 03.5
01C1: MOVWF 07
01C2: BCF 03.5
01C3: BTFSS 07.3
01C4: GOTO 1C3
01C5: NOP
01C6: GOTO 1C7
01C7: NOP
01C8: BSF 20.4
01C9: MOVF 20,W
01CA: BSF 03.5
01CB: MOVWF 07
01CC: NOP
....................
.................... i2c_start();
01CD: BCF 03.5
01CE: BSF 20.4
01CF: MOVF 20,W
01D0: BSF 03.5
01D1: MOVWF 07
01D2: NOP
01D3: BCF 03.5
01D4: BSF 20.3
01D5: MOVF 20,W
01D6: BSF 03.5
01D7: MOVWF 07
01D8: NOP
01D9: BCF 03.5
01DA: BCF 07.4
01DB: BCF 20.4
01DC: MOVF 20,W
01DD: BSF 03.5
01DE: MOVWF 07
01DF: NOP
01E0: BCF 03.5
01E1: BCF 07.3
01E2: BCF 20.3
01E3: MOVF 20,W
01E4: BSF 03.5
01E5: MOVWF 07
.................... I2C_Write(AK_W);
01E6: MOVLW 38
01E7: BCF 03.5
01E8: MOVWF 2F
01E9: CALL 041
.................... I2C_write(L);
01EA: MOVF 2A,W
01EB: MOVWF 2F
01EC: CALL 041
.................... i2c_start();
01ED: BSF 20.4
01EE: MOVF 20,W
01EF: BSF 03.5
01F0: MOVWF 07
01F1: NOP
01F2: BCF 03.5
01F3: BSF 20.3
01F4: MOVF 20,W
01F5: BSF 03.5
01F6: MOVWF 07
01F7: NOP
01F8: BCF 03.5
01F9: BTFSS 07.3
01FA: GOTO 1F9
01FB: BCF 07.4
01FC: BCF 20.4
01FD: MOVF 20,W
01FE: BSF 03.5
01FF: MOVWF 07
0200: NOP
0201: BCF 03.5
0202: BCF 07.3
0203: BCF 20.3
0204: MOVF 20,W
0205: BSF 03.5
0206: MOVWF 07
.................... I2C_Write(AK_R);
0207: MOVLW 39
0208: BCF 03.5
0209: MOVWF 2F
020A: CALL 041
.................... XL=i2c_read(0);
020B: CLRF 77
020C: CALL 130
020D: MOVF 78,W
020E: MOVWF 2B
.................... i2c_stop();
020F: BCF 20.4
0210: MOVF 20,W
0211: BSF 03.5
0212: MOVWF 07
0213: NOP
0214: BCF 03.5
0215: BSF 20.3
0216: MOVF 20,W
0217: BSF 03.5
0218: MOVWF 07
0219: BCF 03.5
021A: BTFSS 07.3
021B: GOTO 21A
021C: NOP
021D: GOTO 21E
021E: NOP
021F: BSF 20.4
0220: MOVF 20,W
0221: BSF 03.5
0222: MOVWF 07
0223: NOP
....................
.................... x = (((unsigned int16) XH << 8) + XL ); //prevod na 16bit hodnotu
0224: BCF 03.5
0225: CLRF 30
0226: MOVF 2C,W
0227: MOVWF 2F
0228: MOVWF 30
0229: CLRF 2F
022A: MOVF 2B,W
022B: ADDWF 2F,W
022C: MOVWF 78
022D: MOVF 30,W
022E: MOVWF 7A
022F: BTFSC 03.0
0230: INCF 7A,F
0231: MOVF 78,W
0232: MOVWF 2D
0233: MOVF 7A,W
0234: MOVWF 2E
.................... x=x/4;
0235: MOVF 2E,W
0236: MOVWF 30
0237: MOVF 2D,W
0238: MOVWF 2F
0239: CLRF 32
023A: MOVLW 04
023B: MOVWF 31
*
0279: MOVF 79,W
027A: MOVWF 2E
027B: MOVF 78,W
027C: MOVWF 2D
....................
.................... return x;
027D: MOVF 2D,W
027E: MOVWF 78
027F: MOVF 2E,W
0280: MOVWF 79
.................... }
0281: RETURN
....................
.................... int16 akX (void)
.................... {
.................... int16 X;
.................... X= akR (AK_XH, AK_XL);
0282: MOVLW 01
0283: MOVWF 29
0284: MOVLW 02
0285: MOVWF 2A
0286: CALL 175
0287: MOVF 79,W
0288: MOVWF 28
0289: MOVF 78,W
028A: MOVWF 27
.................... return X;
028B: MOVF 27,W
028C: MOVWF 78
028D: MOVF 28,W
028E: MOVWF 79
.................... }
028F: BCF 0A.3
0290: BCF 0A.4
0291: GOTO 40E (RETURN)
....................
.................... int16 akY (void)
.................... {
.................... int16 Y;
.................... Y= akR (AK_YH, AK_YL);
0292: MOVLW 03
0293: MOVWF 29
0294: MOVLW 04
0295: MOVWF 2A
0296: CALL 175
0297: MOVF 79,W
0298: MOVWF 28
0299: MOVF 78,W
029A: MOVWF 27
.................... return Y;
029B: MOVF 27,W
029C: MOVWF 78
029D: MOVF 28,W
029E: MOVWF 79
.................... }
029F: BCF 0A.3
02A0: BCF 0A.4
02A1: GOTO 413 (RETURN)
....................
.................... int16 akZ (void)
.................... {
.................... int16 Z;
.................... Z= akR (AK_ZH, AK_ZL);
02A2: MOVLW 05
02A3: MOVWF 29
02A4: MOVLW 06
02A5: MOVWF 2A
02A6: CALL 175
02A7: MOVF 79,W
02A8: MOVWF 28
02A9: MOVF 78,W
02AA: MOVWF 27
.................... return Z;
02AB: MOVF 27,W
02AC: MOVWF 78
02AD: MOVF 28,W
02AE: MOVWF 79
.................... }
02AF: BCF 0A.3
02B0: BCF 0A.4
02B1: GOTO 418 (RETURN)
....................
....................
....................
....................
....................
.................... signed int16 X, Y, Z; //promenne pro akcelerometr
....................
.................... void main()
.................... {
*
0397: CLRF 04
0398: BCF 03.7
0399: MOVLW 1F
039A: ANDWF 03,F
039B: MOVLW 71
039C: BSF 03.5
039D: MOVWF 0F
039E: MOVF 0F,W
039F: BCF 03.5
03A0: BCF 20.7
03A1: MOVF 20,W
03A2: BSF 03.5
03A3: MOVWF 07
03A4: BCF 03.5
03A5: BSF 07.7
03A6: BSF 03.5
03A7: BSF 03.6
03A8: MOVF 09,W
03A9: ANDLW C0
03AA: MOVWF 09
03AB: BCF 03.6
03AC: BCF 1F.4
03AD: BCF 1F.5
03AE: MOVLW 00
03AF: BSF 03.6
03B0: MOVWF 08
03B1: BCF 03.5
03B2: CLRF 07
03B3: CLRF 08
03B4: CLRF 09
.................... setup_adc_ports(NO_ANALOGS|VSS_VDD);
*
03B8: BSF 03.5
03B9: BSF 03.6
03BA: MOVF 09,W
03BB: ANDLW C0
03BC: MOVWF 09
03BD: BCF 03.6
03BE: BCF 1F.4
03BF: BCF 1F.5
03C0: MOVLW 00
03C1: BSF 03.6
03C2: MOVWF 08
.................... setup_adc(ADC_CLOCK_DIV_2);
03C3: BCF 03.5
03C4: BCF 03.6
03C5: BCF 1F.6
03C6: BCF 1F.7
03C7: BSF 03.5
03C8: BCF 1F.7
03C9: BCF 03.5
03CA: BSF 1F.0
.................... setup_spi(SPI_SS_DISABLED);
03CB: BCF 14.5
03CC: BCF 20.5
03CD: MOVF 20,W
03CE: BSF 03.5
03CF: MOVWF 07
03D0: BCF 03.5
03D1: BSF 20.4
03D2: MOVF 20,W
03D3: BSF 03.5
03D4: MOVWF 07
03D5: BCF 03.5
03D6: BCF 20.3
03D7: MOVF 20,W
03D8: BSF 03.5
03D9: MOVWF 07
03DA: MOVLW 01
03DB: BCF 03.5
03DC: MOVWF 14
03DD: MOVLW 00
03DE: BSF 03.5
03DF: MOVWF 14
.................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
03E0: MOVF 01,W
03E1: ANDLW C7
03E2: IORLW 08
03E3: MOVWF 01
.................... setup_timer_1(T1_DISABLED);
03E4: BCF 03.5
03E5: CLRF 10
.................... setup_timer_2(T2_DISABLED,0,1);
03E6: MOVLW 00
03E7: MOVWF 78
03E8: MOVWF 12
03E9: MOVLW 00
03EA: BSF 03.5
03EB: MOVWF 12
.................... setup_ccp1(CCP_OFF);
03EC: BCF 03.5
03ED: BSF 20.2
03EE: MOVF 20,W
03EF: BSF 03.5
03F0: MOVWF 07
03F1: BCF 03.5
03F2: CLRF 17
03F3: BSF 03.5
03F4: CLRF 1B
03F5: CLRF 1C
03F6: MOVLW 01
03F7: MOVWF 1D
.................... setup_comparator(NC_NC_NC_NC);
03F8: BCF 03.5
03F9: BSF 03.6
03FA: CLRF 07
03FB: CLRF 08
03FC: CLRF 09
....................
.................... setAK(); //nastaveni akcelerometru
03FD: BCF 03.6
03FE: GOTO 08B
....................
.................... printf("Akcelerometr IMU01A - MMA8451Q \r\n",);
03FF: MOVLW 04
0400: BSF 03.6
0401: MOVWF 0D
0402: MOVLW 00
0403: MOVWF 0F
0404: BCF 03.6
0405: CALL 0EA
.................... printf("(c) MLAB JACHO 2013 \r\n",);
0406: MOVLW 15
0407: BSF 03.6
0408: MOVWF 0D
0409: MOVLW 00
040A: MOVWF 0F
040B: BCF 03.6
040C: CALL 0EA
....................
.................... while(TRUE)
.................... {
.................... X=akX ();
040D: GOTO 282
040E: MOVF 79,W
040F: MOVWF 22
0410: MOVF 78,W
0411: MOVWF 21
.................... Y=akY ();
0412: GOTO 292
0413: MOVF 79,W
0414: MOVWF 24
0415: MOVF 78,W
0416: MOVWF 23
.................... Z=akZ ();
0417: GOTO 2A2
0418: MOVF 79,W
0419: MOVWF 26
041A: MOVF 78,W
041B: MOVWF 25
.................... printf("Namerene hodnoty: \r\n",);
041C: MOVLW 21
041D: BSF 03.6
041E: MOVWF 0D
041F: MOVLW 00
0420: MOVWF 0F
0421: BCF 03.6
0422: CALL 0EA
.................... printf("Osa X: %Ld \r\n",X);
0423: MOVLW 2C
0424: BSF 03.6
0425: MOVWF 0D
0426: MOVLW 00
0427: MOVWF 0F
0428: BCF 03.0
0429: MOVLW 07
042A: BCF 03.6
042B: MOVWF 27
042C: CALL 2B2
042D: MOVLW 10
042E: MOVWF 04
042F: MOVF 22,W
0430: MOVWF 28
0431: MOVF 21,W
0432: MOVWF 27
0433: CALL 301
0434: MOVLW 20
0435: MOVWF 30
0436: CALL 0C6
0437: MOVLW 0D
0438: MOVWF 30
0439: CALL 0C6
043A: MOVLW 0A
043B: MOVWF 30
043C: CALL 0C6
.................... printf("Osa Y: %Ld \r\n",Y);
043D: MOVLW 33
043E: BSF 03.6
043F: MOVWF 0D
0440: MOVLW 00
0441: MOVWF 0F
0442: BCF 03.0
0443: MOVLW 07
0444: BCF 03.6
0445: MOVWF 27
0446: CALL 2B2
0447: MOVLW 10
0448: MOVWF 04
0449: MOVF 24,W
044A: MOVWF 28
044B: MOVF 23,W
044C: MOVWF 27
044D: CALL 301
044E: MOVLW 20
044F: MOVWF 30
0450: CALL 0C6
0451: MOVLW 0D
0452: MOVWF 30
0453: CALL 0C6
0454: MOVLW 0A
0455: MOVWF 30
0456: CALL 0C6
.................... printf("Osa Z: %Ld \r\n",Z);
0457: MOVLW 3A
0458: BSF 03.6
0459: MOVWF 0D
045A: MOVLW 00
045B: MOVWF 0F
045C: BCF 03.0
045D: MOVLW 07
045E: BCF 03.6
045F: MOVWF 27
0460: CALL 2B2
0461: MOVLW 10
0462: MOVWF 04
0463: MOVF 26,W
0464: MOVWF 28
0465: MOVF 25,W
0466: MOVWF 27
0467: CALL 301
0468: MOVLW 20
0469: MOVWF 30
046A: CALL 0C6
046B: MOVLW 0D
046C: MOVWF 30
046D: CALL 0C6
046E: MOVLW 0A
046F: MOVWF 30
0470: CALL 0C6
.................... delay_ms (2000);
0471: MOVLW 08
0472: MOVWF 27
0473: MOVLW FA
0474: MOVWF 28
0475: GOTO 381
0476: DECFSZ 27,F
0477: GOTO 473
....................
.................... }
0478: GOTO 40D
.................... }
....................
0479: SLEEP
 
Configuration Fuses:
Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG
Word 2: 3FFF NOWRT BORV40