| 1,17 → 1,17 |
|---|
| CCS PCM C Compiler, Version 4.106, 47914 15-V-13 18:53 |
| CCS PCM C Compiler, Version 4.106, 47914 15-V-13 20:34 |
| |
| Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst |
| |
| ROM used: 821 words (10%) |
| ROM used: 775 words (9%) |
| Largest free fragment is 2048 |
| RAM used: 18 (5%) at main() level |
| 27 (7%) worst case |
| RAM used: 28 (8%) at main() level |
| 38 (10%) worst case |
| Stack: 2 locations |
| |
| * |
| 0000: MOVLW 02 |
| 0001: MOVWF 0A |
| 0002: GOTO 250 |
| 0002: GOTO 24F |
| 0003: NOP |
| .................... #include "main.h" |
| .................... #include <16F887.h> |
| 48,7 → 48,7 |
|---|
| 0026: MOVWF 07 |
| 0027: NOP |
| 0028: BCF 03.5 |
| 0029: RLF 32,F |
| 0029: RLF 3D,F |
| 002A: BCF 07.4 |
| 002B: BTFSS 03.0 |
| 002C: GOTO 033 |
| 113,76 → 113,76 |
|---|
| 0067: BCF 03.5 |
| 0068: RETURN |
| * |
| 010C: MOVLW 08 |
| 010D: MOVWF 33 |
| 010E: MOVF 77,W |
| 010F: MOVWF 34 |
| 0110: BSF 20.4 |
| 0111: MOVF 20,W |
| 0112: BSF 03.5 |
| 0113: MOVWF 07 |
| 0114: NOP |
| 0115: BCF 03.5 |
| 0116: BSF 20.3 |
| 0117: MOVF 20,W |
| 0118: BSF 03.5 |
| 0119: MOVWF 07 |
| 011A: BCF 03.5 |
| 011B: BTFSS 07.3 |
| 011C: GOTO 11B |
| 011D: BTFSC 07.4 |
| 011E: BSF 03.0 |
| 011F: BTFSS 07.4 |
| 0120: BCF 03.0 |
| 0121: RLF 78,F |
| 0122: NOP |
| 0123: BCF 20.3 |
| 0124: MOVF 20,W |
| 0125: BSF 03.5 |
| 0126: MOVWF 07 |
| 0127: BCF 03.5 |
| 0128: BCF 07.3 |
| 0129: DECFSZ 33,F |
| 012A: GOTO 110 |
| 012B: BSF 20.4 |
| 012C: MOVF 20,W |
| 012D: BSF 03.5 |
| 012E: MOVWF 07 |
| 012F: NOP |
| 0130: BCF 03.5 |
| 0131: BCF 07.4 |
| 0132: MOVF 34,W |
| 0133: BTFSC 03.2 |
| 0134: GOTO 13A |
| 0135: BCF 20.4 |
| 0136: MOVF 20,W |
| 0137: BSF 03.5 |
| 0138: MOVWF 07 |
| 0139: BCF 03.5 |
| 013A: NOP |
| 013B: BSF 20.3 |
| 013C: MOVF 20,W |
| 013D: BSF 03.5 |
| 013E: MOVWF 07 |
| 013F: BCF 03.5 |
| 0140: BTFSS 07.3 |
| 0141: GOTO 140 |
| 0142: NOP |
| 0143: BCF 07.3 |
| 0144: BCF 20.3 |
| 0145: MOVF 20,W |
| 0146: BSF 03.5 |
| 0147: MOVWF 07 |
| 0148: NOP |
| 0149: BCF 03.5 |
| 014A: BCF 07.4 |
| 014B: BCF 20.4 |
| 014C: MOVF 20,W |
| 014D: BSF 03.5 |
| 014E: MOVWF 07 |
| 014F: BCF 03.5 |
| 0150: RETURN |
| .................... #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD |
| 010B: MOVLW 08 |
| 010C: MOVWF 3E |
| 010D: MOVF 77,W |
| 010E: MOVWF 3F |
| 010F: BSF 20.4 |
| 0110: MOVF 20,W |
| 0111: BSF 03.5 |
| 0112: MOVWF 07 |
| 0113: NOP |
| 0114: BCF 03.5 |
| 0115: BSF 20.3 |
| 0116: MOVF 20,W |
| 0117: BSF 03.5 |
| 0118: MOVWF 07 |
| 0119: BCF 03.5 |
| 011A: BTFSS 07.3 |
| 011B: GOTO 11A |
| 011C: BTFSC 07.4 |
| 011D: BSF 03.0 |
| 011E: BTFSS 07.4 |
| 011F: BCF 03.0 |
| 0120: RLF 78,F |
| 0121: NOP |
| 0122: BCF 20.3 |
| 0123: MOVF 20,W |
| 0124: BSF 03.5 |
| 0125: MOVWF 07 |
| 0126: BCF 03.5 |
| 0127: BCF 07.3 |
| 0128: DECFSZ 3E,F |
| 0129: GOTO 10F |
| 012A: BSF 20.4 |
| 012B: MOVF 20,W |
| 012C: BSF 03.5 |
| 012D: MOVWF 07 |
| 012E: NOP |
| 012F: BCF 03.5 |
| 0130: BCF 07.4 |
| 0131: MOVF 3F,W |
| 0132: BTFSC 03.2 |
| 0133: GOTO 139 |
| 0134: BCF 20.4 |
| 0135: MOVF 20,W |
| 0136: BSF 03.5 |
| 0137: MOVWF 07 |
| 0138: BCF 03.5 |
| 0139: NOP |
| 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: NOP |
| 0142: BCF 07.3 |
| 0143: BCF 20.3 |
| 0144: MOVF 20,W |
| 0145: BSF 03.5 |
| 0146: MOVWF 07 |
| 0147: NOP |
| 0148: BCF 03.5 |
| 0149: BCF 07.4 |
| 014A: BCF 20.4 |
| 014B: MOVF 20,W |
| 014C: BSF 03.5 |
| 014D: MOVWF 07 |
| 014E: BCF 03.5 |
| 014F: RETURN |
| .................... #use rs232(baud=115200,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD |
| * |
| 00A2: BCF 20.7 |
| 00A3: MOVF 20,W |
| 195,31 → 195,30 |
|---|
| 00AA: GOTO 0AB |
| 00AB: NOP |
| 00AC: BSF 78.7 |
| 00AD: GOTO 0BC |
| 00AD: GOTO 0C0 |
| 00AE: BCF 78.7 |
| 00AF: RRF 35,F |
| 00B0: BTFSC 03.0 |
| 00B1: BSF 07.7 |
| 00B2: BTFSS 03.0 |
| 00B3: BCF 07.7 |
| 00B4: BSF 78.6 |
| 00B5: GOTO 0BC |
| 00B6: BCF 78.6 |
| 00B7: DECFSZ 78,F |
| 00B8: GOTO 0AF |
| 00B9: GOTO 0BA |
| 00BA: NOP |
| 00BB: BSF 07.7 |
| 00BC: MOVLW 3F |
| 00BD: MOVWF 04 |
| 00BE: DECFSZ 04,F |
| 00BF: GOTO 0BE |
| 00C0: NOP |
| 00C1: BTFSC 78.7 |
| 00C2: GOTO 0AE |
| 00C3: BTFSC 78.6 |
| 00C4: GOTO 0B6 |
| 00C5: RETURN |
| 00AF: MOVF 40,W |
| 00B0: MOVWF 79 |
| 00B1: RRF 79,F |
| 00B2: BTFSC 03.0 |
| 00B3: BSF 07.7 |
| 00B4: BTFSS 03.0 |
| 00B5: BCF 07.7 |
| 00B6: BSF 78.6 |
| 00B7: GOTO 0C0 |
| 00B8: BCF 78.6 |
| 00B9: DECFSZ 78,F |
| 00BA: GOTO 0B1 |
| 00BB: MOVF 79,W |
| 00BC: MOVWF 40 |
| 00BD: GOTO 0BE |
| 00BE: NOP |
| 00BF: BSF 07.7 |
| 00C0: BTFSC 78.7 |
| 00C1: GOTO 0AE |
| 00C2: BTFSC 78.6 |
| 00C3: GOTO 0B8 |
| 00C4: RETURN |
| .................... |
| .................... |
| .................... #include "HMC5883L.h" |
| 284,15 → 283,15 |
|---|
| .................... i2c_write(HMC5883L_WRT_ADDR); |
| 0081: MOVLW 3C |
| 0082: BCF 03.5 |
| 0083: MOVWF 32 |
| 0083: MOVWF 3D |
| 0084: CALL 01F |
| .................... i2c_write(reg); |
| 0085: MOVF 2C,W |
| 0086: MOVWF 32 |
| 0085: MOVF 37,W |
| 0086: MOVWF 3D |
| 0087: CALL 01F |
| .................... i2c_write(data); |
| 0088: MOVF 2D,W |
| 0089: MOVWF 32 |
| 0088: MOVF 38,W |
| 0089: MOVWF 3D |
| 008A: CALL 01F |
| .................... i2c_stop(); |
| 008B: BCF 20.4 |
| 348,12 → 347,12 |
|---|
| .................... // from the HMC5883L x,y,z registers. |
| .................... hmc5883l_result compass = {0,0,0}; |
| * |
| 0271: CLRF 21 |
| 0272: CLRF 22 |
| 0273: CLRF 23 |
| 0274: CLRF 24 |
| 0275: CLRF 25 |
| 0276: CLRF 26 |
| 0270: CLRF 21 |
| 0271: CLRF 22 |
| 0272: CLRF 23 |
| 0273: CLRF 24 |
| 0274: CLRF 25 |
| 0275: CLRF 26 |
| .................... |
| .................... //------------------------------ |
| .................... void hmc5883l_read_data(void) |
| 369,196 → 368,2369 |
|---|
| .................... |
| .................... i2c_start(); |
| * |
| 0151: BSF 20.4 |
| 0152: MOVF 20,W |
| 0153: BSF 03.5 |
| 0154: MOVWF 07 |
| 0155: NOP |
| 0156: BCF 03.5 |
| 0157: BSF 20.3 |
| 0158: MOVF 20,W |
| 0159: BSF 03.5 |
| 015A: MOVWF 07 |
| 015B: NOP |
| 015C: BCF 03.5 |
| 015D: BCF 07.4 |
| 015E: BCF 20.4 |
| 015F: MOVF 20,W |
| 0160: BSF 03.5 |
| 0161: MOVWF 07 |
| 0162: NOP |
| 0163: BCF 03.5 |
| 0164: BCF 07.3 |
| 0165: BCF 20.3 |
| 0166: MOVF 20,W |
| 0167: BSF 03.5 |
| 0168: MOVWF 07 |
| 0150: BSF 20.4 |
| 0151: MOVF 20,W |
| 0152: BSF 03.5 |
| 0153: MOVWF 07 |
| 0154: NOP |
| 0155: BCF 03.5 |
| 0156: BSF 20.3 |
| 0157: MOVF 20,W |
| 0158: BSF 03.5 |
| 0159: MOVWF 07 |
| 015A: NOP |
| 015B: BCF 03.5 |
| 015C: BCF 07.4 |
| 015D: BCF 20.4 |
| 015E: MOVF 20,W |
| 015F: BSF 03.5 |
| 0160: MOVWF 07 |
| 0161: NOP |
| 0162: BCF 03.5 |
| 0163: BCF 07.3 |
| 0164: BCF 20.3 |
| 0165: MOVF 20,W |
| 0166: BSF 03.5 |
| 0167: MOVWF 07 |
| .................... i2c_write(HMC5883L_WRT_ADDR); |
| 0169: MOVLW 3C |
| 016A: BCF 03.5 |
| 016B: MOVWF 32 |
| 016C: CALL 01F |
| 0168: MOVLW 3C |
| 0169: BCF 03.5 |
| 016A: MOVWF 3D |
| 016B: CALL 01F |
| .................... i2c_write(HMC5883L_X_MSB_REG); // Point to X-msb register |
| 016D: MOVLW 03 |
| 016E: MOVWF 32 |
| 016F: CALL 01F |
| 016C: MOVLW 03 |
| 016D: MOVWF 3D |
| 016E: CALL 01F |
| .................... i2c_start(); |
| 0170: BSF 20.4 |
| 0171: MOVF 20,W |
| 0172: BSF 03.5 |
| 0173: MOVWF 07 |
| 0174: NOP |
| 0175: BCF 03.5 |
| 0176: BSF 20.3 |
| 0177: MOVF 20,W |
| 0178: BSF 03.5 |
| 0179: MOVWF 07 |
| 017A: NOP |
| 017B: BCF 03.5 |
| 017C: BTFSS 07.3 |
| 017D: GOTO 17C |
| 017E: BCF 07.4 |
| 017F: BCF 20.4 |
| 0180: MOVF 20,W |
| 0181: BSF 03.5 |
| 0182: MOVWF 07 |
| 0183: NOP |
| 0184: BCF 03.5 |
| 0185: BCF 07.3 |
| 0186: BCF 20.3 |
| 0187: MOVF 20,W |
| 0188: BSF 03.5 |
| 0189: MOVWF 07 |
| 016F: BSF 20.4 |
| 0170: MOVF 20,W |
| 0171: BSF 03.5 |
| 0172: MOVWF 07 |
| 0173: NOP |
| 0174: BCF 03.5 |
| 0175: BSF 20.3 |
| 0176: MOVF 20,W |
| 0177: BSF 03.5 |
| 0178: MOVWF 07 |
| 0179: NOP |
| 017A: BCF 03.5 |
| 017B: BTFSS 07.3 |
| 017C: GOTO 17B |
| 017D: BCF 07.4 |
| 017E: BCF 20.4 |
| 017F: MOVF 20,W |
| 0180: BSF 03.5 |
| 0181: MOVWF 07 |
| 0182: NOP |
| 0183: BCF 03.5 |
| 0184: BCF 07.3 |
| 0185: BCF 20.3 |
| 0186: MOVF 20,W |
| 0187: BSF 03.5 |
| 0188: MOVWF 07 |
| .................... i2c_write(HMC5883L_READ_ADDR); |
| 018A: MOVLW 3D |
| 018B: BCF 03.5 |
| 018C: MOVWF 32 |
| 018D: CALL 01F |
| 0189: MOVLW 3D |
| 018A: BCF 03.5 |
| 018B: MOVWF 3D |
| 018C: CALL 01F |
| .................... |
| .................... x_msb = i2c_read(); |
| 018E: MOVLW 01 |
| 018F: MOVWF 77 |
| 0190: CALL 10C |
| 0191: MOVF 78,W |
| 0192: MOVWF 2D |
| 018D: MOVLW 01 |
| 018E: MOVWF 77 |
| 018F: CALL 10B |
| 0190: MOVF 78,W |
| 0191: MOVWF 38 |
| .................... x_lsb = i2c_read(); |
| 0193: MOVLW 01 |
| 0194: MOVWF 77 |
| 0195: CALL 10C |
| 0196: MOVF 78,W |
| 0197: MOVWF 2C |
| 0192: MOVLW 01 |
| 0193: MOVWF 77 |
| 0194: CALL 10B |
| 0195: MOVF 78,W |
| 0196: MOVWF 37 |
| .................... |
| .................... z_msb = i2c_read(); |
| 0198: MOVLW 01 |
| 0199: MOVWF 77 |
| 019A: CALL 10C |
| 019B: MOVF 78,W |
| 019C: MOVWF 31 |
| 0197: MOVLW 01 |
| 0198: MOVWF 77 |
| 0199: CALL 10B |
| 019A: MOVF 78,W |
| 019B: MOVWF 3C |
| .................... z_lsb = i2c_read(); |
| 019D: MOVLW 01 |
| 019E: MOVWF 77 |
| 019F: CALL 10C |
| 01A0: MOVF 78,W |
| 01A1: MOVWF 30 |
| 019C: MOVLW 01 |
| 019D: MOVWF 77 |
| 019E: CALL 10B |
| 019F: MOVF 78,W |
| 01A0: MOVWF 3B |
| .................... |
| .................... y_msb = i2c_read(); |
| 01A2: MOVLW 01 |
| 01A3: MOVWF 77 |
| 01A4: CALL 10C |
| 01A5: MOVF 78,W |
| 01A6: MOVWF 2F |
| 01A1: MOVLW 01 |
| 01A2: MOVWF 77 |
| 01A3: CALL 10B |
| 01A4: MOVF 78,W |
| 01A5: MOVWF 3A |
| .................... y_lsb = i2c_read(0); // do a NACK on last read |
| 01A7: CLRF 77 |
| 01A8: CALL 10C |
| 01A9: MOVF 78,W |
| 01AA: MOVWF 2E |
| 01A6: CLRF 77 |
| 01A7: CALL 10B |
| 01A8: MOVF 78,W |
| 01A9: MOVWF 39 |
| .................... |
| .................... i2c_stop(); |
| 01AB: BCF 20.4 |
| 01AC: MOVF 20,W |
| 01AD: BSF 03.5 |
| 01AE: MOVWF 07 |
| 01AF: NOP |
| 01B0: BCF 03.5 |
| 01B1: BSF 20.3 |
| 01B2: MOVF 20,W |
| 01B3: BSF 03.5 |
| 01B4: MOVWF 07 |
| 01B5: BCF 03.5 |
| 01B6: BTFSS 07.3 |
| 01B7: GOTO 1B6 |
| 01B8: NOP |
| 01B9: GOTO 1BA |
| 01BA: NOP |
| 01BB: BSF 20.4 |
| 01BC: MOVF 20,W |
| 01BD: BSF 03.5 |
| 01BE: MOVWF 07 |
| 01BF: NOP |
| 01AA: BCF 20.4 |
| 01AB: MOVF 20,W |
| 01AC: BSF 03.5 |
| 01AD: MOVWF 07 |
| 01AE: NOP |
| 01AF: BCF 03.5 |
| 01B0: BSF 20.3 |
| 01B1: MOVF 20,W |
| 01B2: BSF 03.5 |
| 01B3: MOVWF 07 |
| 01B4: BCF 03.5 |
| 01B5: BTFSS 07.3 |
| 01B6: GOTO 1B5 |
| 01B7: NOP |
| 01B8: GOTO 1B9 |
| 01B9: NOP |
| 01BA: BSF 20.4 |
| 01BB: MOVF 20,W |
| 01BC: BSF 03.5 |
| 01BD: MOVWF 07 |
| 01BE: NOP |
| .................... |
| .................... // Combine high and low bytes into 16-bit values. |
| .................... compass.x = make16(x_msb, x_lsb); |
| 01C0: BCF 03.5 |
| 01C1: MOVF 2D,W |
| 01C2: MOVWF 22 |
| 01C3: MOVF 2C,W |
| 01C4: MOVWF 21 |
| 01BF: BCF 03.5 |
| 01C0: MOVF 38,W |
| 01C1: MOVWF 22 |
| 01C2: MOVF 37,W |
| 01C3: MOVWF 21 |
| .................... compass.y = make16(y_msb, y_lsb); |
| 01C5: MOVF 2F,W |
| 01C6: MOVWF 24 |
| 01C7: MOVF 2E,W |
| 01C8: MOVWF 23 |
| 01C4: MOVF 3A,W |
| 01C5: MOVWF 24 |
| 01C6: MOVF 39,W |
| 01C7: MOVWF 23 |
| .................... compass.z = make16(z_msb, z_lsb); |
| 01C9: MOVF 31,W |
| 01CA: MOVWF 26 |
| 01CB: MOVF 30,W |
| 01CC: MOVWF 25 |
| 01C8: MOVF 3C,W |
| 01C9: MOVWF 26 |
| 01CA: MOVF 3B,W |
| 01CB: MOVWF 25 |
| .................... } |
| 01CD: BCF 0A.3 |
| 01CE: BCF 0A.4 |
| 01CF: GOTO 2EE (RETURN) |
| 01CC: BCF 0A.3 |
| 01CD: BCF 0A.4 |
| 01CE: GOTO 2E1 (RETURN) |
| .................... |
| .................... |
| .................... |
| .................... |
| .................... #include <math.h> |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... //// (C) Copyright 1996,2008 Custom Computer Services //// |
| .................... //// This source code may only be used by licensed users of the CCS C //// |
| .................... //// compiler. This source code may only be distributed to other //// |
| .................... //// licensed users of the CCS C compiler. No other use, reproduction //// |
| .................... //// or distribution is permitted without written permission. //// |
| .................... //// Derivative programs created using this software in object code //// |
| .................... //// form are not restricted in any way. //// |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... //// //// |
| .................... //// History: //// |
| .................... //// * 9/20/2001 : Improvments are made to sin/cos code. //// |
| .................... //// The code now is small, much faster, //// |
| .................... //// and more accurate. //// |
| .................... //// * 2/21/2007 : Compiler handles & operator differently and does |
| .................... //// not return generic (int8 *) so type cast is done //// |
| .................... //// //// |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... |
| .................... #ifndef MATH_H |
| .................... #define MATH_H |
| .................... |
| .................... #ifdef PI |
| .................... #undef PI |
| .................... #endif |
| .................... #define PI 3.1415926535897932 |
| .................... |
| .................... |
| .................... #define SQRT2 1.4142135623730950 |
| .................... |
| .................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279}; |
| .................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718}; |
| .................... |
| .................... ///////////////////////////// Round Functions ////////////////////////////// |
| .................... |
| .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n) |
| .................... { |
| .................... float32 y, res; |
| .................... unsigned int16 l; |
| .................... int1 s; |
| .................... |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y <= 32768.0) |
| .................... res = (float32)(unsigned int16)y; |
| .................... |
| .................... else if (y < 10000000.0) |
| .................... { |
| .................... l = (unsigned int16)(y/32768.0); |
| .................... y = 32768.0*(y/32768.0 - (float32)l); |
| .................... res = 32768.0*(float32)l; |
| .................... res += (float32)(unsigned int16)y; |
| .................... } |
| .................... |
| .................... else |
| .................... res = y; |
| .................... |
| .................... y = y - (float32)(unsigned int16)y; |
| .................... |
| .................... if (s) |
| .................... res = -res; |
| .................... |
| .................... if (y != 0) |
| .................... { |
| .................... if (s == 1 && n == 0) |
| .................... res -= 1.0; |
| .................... |
| .................... if (s == 0 && n == 1) |
| .................... res += 1.0; |
| .................... } |
| .................... if (x == 0) |
| .................... res = 0; |
| .................... |
| .................... return (res); |
| .................... } |
| .................... |
| .................... // Overloaded Functions to take care for new Data types in PCD |
| .................... // Overloaded function CEIL_FLOOR() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n) |
| .................... { |
| .................... float48 y, res; |
| .................... unsigned int16 l; |
| .................... int1 s; |
| .................... |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y <= 32768.0) |
| .................... res = (float48)(unsigned int16)y; |
| .................... |
| .................... else if (y < 10000000.0) |
| .................... { |
| .................... l = (unsigned int16)(y/32768.0); |
| .................... y = 32768.0*(y/32768.0 - (float48)l); |
| .................... res = 32768.0*(float32)l; |
| .................... res += (float48)(unsigned int16)y; |
| .................... } |
| .................... |
| .................... else |
| .................... res = y; |
| .................... |
| .................... y = y - (float48)(unsigned int16)y; |
| .................... |
| .................... if (s) |
| .................... res = -res; |
| .................... |
| .................... if (y != 0) |
| .................... { |
| .................... if (s == 1 && n == 0) |
| .................... res -= 1.0; |
| .................... |
| .................... if (s == 0 && n == 1) |
| .................... res += 1.0; |
| .................... } |
| .................... if (x == 0) |
| .................... res = 0; |
| .................... |
| .................... return (res); |
| .................... } |
| .................... |
| .................... |
| .................... // Overloaded function CEIL_FLOOR() for data type - Float64 |
| .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n) |
| .................... { |
| .................... float64 y, res; |
| .................... unsigned int16 l; |
| .................... int1 s; |
| .................... |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y <= 32768.0) |
| .................... res = (float64)(unsigned int16)y; |
| .................... |
| .................... else if (y < 10000000.0) |
| .................... { |
| .................... l = (unsigned int16)(y/32768.0); |
| .................... y = 32768.0*(y/32768.0 - (float64)l); |
| .................... res = 32768.0*(float64)l; |
| .................... res += (float64)(unsigned int16)y; |
| .................... } |
| .................... |
| .................... else |
| .................... res = y; |
| .................... |
| .................... y = y - (float64)(unsigned int16)y; |
| .................... |
| .................... if (s) |
| .................... res = -res; |
| .................... |
| .................... if (y != 0) |
| .................... { |
| .................... if (s == 1 && n == 0) |
| .................... res -= 1.0; |
| .................... |
| .................... if (s == 0 && n == 1) |
| .................... res += 1.0; |
| .................... } |
| .................... if (x == 0) |
| .................... res = 0; |
| .................... |
| .................... return (res); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float floor(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : rounds down the number x. |
| .................... // Date : N/A |
| .................... // |
| .................... float32 floor(float32 x) |
| .................... { |
| .................... return CEIL_FLOOR(x, 0); |
| .................... } |
| .................... // Following 2 functions are overloaded functions of floor() for PCD |
| .................... // Overloaded function floor() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 floor(float48 x) |
| .................... { |
| .................... return CEIL_FLOOR(x, 0); |
| .................... } |
| .................... |
| .................... // Overloaded function floor() for data type - Float64 |
| .................... float64 floor(float64 x) |
| .................... { |
| .................... return CEIL_FLOOR(x, 0); |
| .................... } |
| .................... #endif |
| .................... |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float ceil(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : rounds up the number x. |
| .................... // Date : N/A |
| .................... // |
| .................... float32 ceil(float32 x) |
| .................... { |
| .................... return CEIL_FLOOR(x, 1); |
| .................... } |
| .................... // Following 2 functions are overloaded functions of ceil() for PCD |
| .................... // Overloaded function ceil() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 ceil(float48 x) |
| .................... { |
| .................... return CEIL_FLOOR(x, 1); |
| .................... } |
| .................... |
| .................... // Overloaded function ceil() for data type - Float64 |
| .................... float64 ceil(float64 x) |
| .................... { |
| .................... return CEIL_FLOOR(x, 1); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float fabs(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : Computes the absolute value of floating point number x |
| .................... // Returns : returns the absolute value of x |
| .................... // Date : N/A |
| .................... // |
| .................... #define fabs abs |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float fmod(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : Computes the floating point remainder of x/y |
| .................... // Returns : returns the value of x= i*y, for some integer i such that, if y |
| .................... // is non zero, the result has the same isgn of x na dmagnitude less than the |
| .................... // magnitude of y. If y is zero then a domain error occurs. |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float fmod(float32 x,float32 y) |
| .................... { |
| .................... float32 i; |
| .................... if (y!=0.0) |
| .................... { |
| .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); |
| .................... return(x-(i*y)); |
| .................... } |
| .................... else |
| .................... { |
| .................... #ifdef _ERRNO |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... } |
| .................... } |
| .................... //Overloaded function for fmod() for PCD |
| .................... // Overloaded function fmod() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 fmod(float48 x,float48 y) |
| .................... { |
| .................... float48 i; |
| .................... if (y!=0.0) |
| .................... { |
| .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); |
| .................... return(x-(i*y)); |
| .................... } |
| .................... else |
| .................... { |
| .................... #ifdef _ERRNO |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... } |
| .................... } |
| .................... // Overloaded function fmod() for data type - Float64 |
| .................... float64 fmod(float64 x,float64 y) |
| .................... { |
| .................... float64 i; |
| .................... if (y!=0.0) |
| .................... { |
| .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); |
| .................... return(x-(i*y)); |
| .................... } |
| .................... else |
| .................... { |
| .................... #ifdef _ERRNO |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... } |
| .................... } |
| .................... #endif |
| .................... //////////////////// Exponential and logarithmic functions //////////////////// |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float exp(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the value (e^x) |
| .................... // Date : N/A |
| .................... // |
| .................... #define LN2 0.6931471805599453 |
| .................... |
| .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, |
| .................... 0.0554965651, 0.240227138, 0.693147172}; |
| .................... |
| .................... |
| .................... float32 exp(float32 x) |
| .................... { |
| .................... float32 y, res, r; |
| .................... #if defined(__PCD__) |
| .................... int8 data1; |
| .................... #endif |
| .................... signed int8 n; |
| .................... int1 s; |
| .................... #ifdef _ERRNO |
| .................... if(x > 88.722838) |
| .................... { |
| .................... errno=ERANGE; |
| .................... return(0); |
| .................... } |
| .................... #endif |
| .................... n = (signed int16)(x/LN2); |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... n = -n; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... res = 0.0; |
| .................... #if !defined(__PCD__) |
| .................... *((unsigned int8 *)(&res)) = n + 0x7F; |
| .................... #endif |
| .................... |
| .................... #if defined(__PCD__) // Takes care of IEEE format for PCD |
| .................... data1 = n+0x7F; |
| .................... if(bit_test(data1,0)) |
| .................... bit_set(*(((unsigned int8 *)(&res)+2)),7); |
| .................... rotate_right(&data1,1); |
| .................... bit_clear(data1,7); |
| .................... *(((unsigned int8 *)(&res)+3)) = data1; |
| .................... #endif |
| .................... |
| .................... y = y/LN2 - (float32)n; |
| .................... |
| .................... r = pe[0]*y + pe[1]; |
| .................... r = r*y + pe[2]; |
| .................... r = r*y + pe[3]; |
| .................... r = r*y + pe[4]; |
| .................... r = r*y + pe[5]; |
| .................... |
| .................... res = res*(1.0 + y*r); |
| .................... |
| .................... if (s) |
| .................... res = 1.0/res; |
| .................... return(res); |
| .................... } |
| .................... |
| .................... |
| .................... //Overloaded function for exp() for PCD |
| .................... // Overloaded function exp() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 exp(float48 x) |
| .................... { |
| .................... float48 y, res, r; |
| .................... int8 data1; |
| .................... signed int8 n; |
| .................... int1 s; |
| .................... #ifdef _ERRNO |
| .................... if(x > 88.722838) |
| .................... { |
| .................... errno=ERANGE; |
| .................... return(0); |
| .................... } |
| .................... #endif |
| .................... n = (signed int16)(x/LN2); |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... n = -n; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... res = 0.0; |
| .................... |
| .................... data1 = n+0x7F; |
| .................... if(bit_test(data1,0)) |
| .................... bit_set(*(((unsigned int8 *)(&res)+4)),7); |
| .................... rotate_right(&data1,1); |
| .................... bit_clear(data1,7); |
| .................... *(((unsigned int8 *)(&res)+5)) = data1; |
| .................... |
| .................... y = y/LN2 - (float48)n; |
| .................... |
| .................... r = pe[0]*y + pe[1]; |
| .................... r = r*y + pe[2]; |
| .................... r = r*y + pe[3]; |
| .................... r = r*y + pe[4]; |
| .................... r = r*y + pe[5]; |
| .................... |
| .................... res = res*(1.0 + y*r); |
| .................... |
| .................... if (s) |
| .................... res = 1.0/res; |
| .................... return(res); |
| .................... } |
| .................... |
| .................... // Overloaded function exp() for data type - Float64 |
| .................... float64 exp(float64 x) |
| .................... { |
| .................... float64 y, res, r; |
| .................... unsigned int16 data1, data2; |
| .................... unsigned int16 *p; |
| .................... signed int16 n; |
| .................... int1 s; |
| .................... #ifdef _ERRNO |
| .................... if(x > 709.7827128) |
| .................... { |
| .................... errno=ERANGE; |
| .................... return(0); |
| .................... } |
| .................... #endif |
| .................... n = (signed int16)(x/LN2); |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... n = -n; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... res = 0.0; |
| .................... |
| .................... #if !defined(__PCD__) |
| .................... *((unsigned int16 *)(&res)) = n + 0x7F; |
| .................... #endif |
| .................... p= (((unsigned int16 *)(&res))+3); |
| .................... data1 = *p; |
| .................... data2 = *p; |
| .................... data1 = n + 0x3FF; |
| .................... data1 = data1 <<4; |
| .................... if(bit_test(data2,15)) |
| .................... bit_set(data1,15); |
| .................... data2 = data2 & 0x000F; |
| .................... data1 ^= data2; |
| .................... |
| .................... *(((unsigned int16 *)(&res)+3)) = data1; |
| .................... |
| .................... |
| .................... y = y/LN2 - (float64)n; |
| .................... |
| .................... r = pe[0]*y + pe[1]; |
| .................... r = r*y + pe[2]; |
| .................... r = r*y + pe[3]; |
| .................... r = r*y + pe[4]; |
| .................... r = r*y + pe[5]; |
| .................... |
| .................... res = res*(1.0 + y*r); |
| .................... |
| .................... if (s) |
| .................... res = 1.0/res; |
| .................... return(res); |
| .................... } |
| .................... |
| .................... #ENDIF |
| .................... |
| .................... |
| .................... /************************************************************/ |
| .................... |
| .................... float32 const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; |
| .................... float32 const ql[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float log(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the the natural log of x |
| .................... // Date : N/A |
| .................... // |
| .................... float32 log(float32 x) |
| .................... { |
| .................... float32 y, res, r, y2; |
| .................... #if defined(__PCD__) |
| .................... unsigned int8 data1,data2; |
| .................... #endif |
| .................... signed int8 n; |
| .................... #ifdef _ERRNO |
| .................... if(x <0) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... if(x ==0) |
| .................... { |
| .................... errno=ERANGE; |
| .................... return(0); |
| .................... } |
| .................... #endif |
| .................... y = x; |
| .................... |
| .................... if (y != 1.0) |
| .................... { |
| .................... #if !defined(__PCD__) |
| .................... *((unsigned int8 *)(&y)) = 0x7E; |
| .................... #endif |
| .................... |
| .................... #if defined(__PCD__) // Takes care of IEEE format |
| .................... data2 = *(((unsigned int8 *)(&y))+3); |
| .................... *(((unsigned int8 *)(&y))+3) = 0x3F; |
| .................... data1 = *(((unsigned int8 *)(&y))+2); |
| .................... bit_clear(data1,7); |
| .................... *(((unsigned int8 *)(&y))+2) = data1; |
| .................... if(bit_test(data2,7)) |
| .................... bit_set(*(((unsigned int8 *)(&y))+3),7); |
| .................... #endif |
| .................... |
| .................... y = (y - 1.0)/(y + 1.0); |
| .................... |
| .................... y2=y*y; |
| .................... |
| .................... res = pl[0]*y2 + pl[1]; |
| .................... res = res*y2 + pl[2]; |
| .................... res = res*y2 + pl[3]; |
| .................... |
| .................... r = ql[0]*y2 + ql[1]; |
| .................... r = r*y2 + ql[2]; |
| .................... r = r*y2 + ql[3]; |
| .................... |
| .................... res = y*res/r; |
| .................... #if !defined(__PCD__) |
| .................... n = *((unsigned int8 *)(&x)) - 0x7E; |
| .................... #endif |
| .................... #if defined(__PCD__) |
| .................... data1 = *(((unsigned int8 *)(&x)+3)); |
| .................... rotate_left(&data1,1); |
| .................... data2 = *(((unsigned int8 *)(&x)+2)); |
| .................... if(bit_test (data2,7)) |
| .................... bit_set(data1,0); |
| .................... n = data1 - 0x7E; |
| .................... #endif |
| .................... |
| .................... if (n<0) |
| .................... r = -(float32)-n; |
| .................... else |
| .................... r = (float32)n; |
| .................... |
| .................... res += r*LN2; |
| .................... } |
| .................... |
| .................... else |
| .................... res = 0.0; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... |
| .................... //Overloaded function for log() for PCD |
| .................... // Overloaded function log() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 log(float48 x) |
| .................... { |
| .................... float48 y, res, r, y2; |
| .................... unsigned int8 data1,data2; |
| .................... signed int8 n; |
| .................... #ifdef _ERRNO |
| .................... if(x <0) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... if(x ==0) |
| .................... { |
| .................... errno=ERANGE; |
| .................... return(0); |
| .................... } |
| .................... #endif |
| .................... y = x; |
| .................... |
| .................... if (y != 1.0) |
| .................... { |
| .................... |
| .................... #if !defined(__PCD__) |
| .................... *((unsigned int8 *)(&y)) = 0x7E; |
| .................... #endif |
| .................... data2 = *(((unsigned int8 *)(&y))+5); |
| .................... *(((unsigned int8 *)(&y))+5) = 0x3F; |
| .................... data1 = *(((unsigned int8 *)(&y))+4); |
| .................... bit_clear(data1,7); |
| .................... *(((unsigned int8 *)(&y))+4) = data1; |
| .................... |
| .................... if(bit_test(data2,7)) |
| .................... bit_set(*(((unsigned int8 *)(&y))+4),7); |
| .................... y = (y - 1.0)/(y + 1.0); |
| .................... |
| .................... y2=y*y; |
| .................... |
| .................... res = pl[0]*y2 + pl[1]; |
| .................... res = res*y2 + pl[2]; |
| .................... res = res*y2 + pl[3]; |
| .................... |
| .................... r = ql[0]*y2 + ql[1]; |
| .................... r = r*y2 + ql[2]; |
| .................... r = r*y2 + ql[3]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... data1 = *(((unsigned int8 *)(&x)+5)); |
| .................... rotate_left(&data1,1); |
| .................... data2 = *(((unsigned int8 *)(&x)+4)); |
| .................... if(bit_test (data2,7)) |
| .................... bit_set(data1,0); |
| .................... |
| .................... n = data1 - 0x7E; |
| .................... |
| .................... if (n<0) |
| .................... r = -(float48)-n; |
| .................... else |
| .................... r = (float48)n; |
| .................... |
| .................... res += r*LN2; |
| .................... } |
| .................... |
| .................... else |
| .................... res = 0.0; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... |
| .................... // Overloaded function log() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float32 const pl_64[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; |
| .................... float32 const ql_64[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; |
| .................... #endif |
| .................... float64 log(float64 x) |
| .................... { |
| .................... float64 y, res, r, y2; |
| .................... unsigned int16 data1,data2; |
| .................... unsigned int16 *p; |
| .................... signed int16 n; |
| .................... #ifdef _ERRNO |
| .................... if(x <0) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... if(x ==0) |
| .................... { |
| .................... errno=ERANGE; |
| .................... return(0); |
| .................... } |
| .................... #endif |
| .................... y = x; |
| .................... |
| .................... if (y != 1.0) |
| .................... { |
| .................... #if !defined(__PCD__) |
| .................... *((unsigned int8 *)(&y)) = 0x7E; |
| .................... #endif |
| .................... p= (((unsigned int16 *)(&y))+3); |
| .................... data1 = *p; |
| .................... data2 = *p; |
| .................... data1 = 0x3FE; |
| .................... data1 = data1 <<4; |
| .................... if(bit_test (data2,15)) |
| .................... bit_set(data1,15); |
| .................... data2 = data2 & 0x000F; |
| .................... data1 ^=data2; |
| .................... |
| .................... *p = data1; |
| .................... |
| .................... y = (y - 1.0)/(y + 1.0); |
| .................... |
| .................... y2=y*y; |
| .................... |
| .................... res = pl_64[0]*y2 + pl_64[1]; |
| .................... res = res*y2 + pl_64[2]; |
| .................... res = res*y2 + pl_64[3]; |
| .................... |
| .................... r = ql_64[0]*y2 + ql_64[1]; |
| .................... r = r*y2 + ql_64[2]; |
| .................... r = r*y2 + ql_64[3]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... p= (((unsigned int16 *)(&x))+3); |
| .................... data1 = *p; |
| .................... bit_clear(data1,15); |
| .................... data1 = data1 >>4; |
| .................... n = data1 - 0x3FE; |
| .................... |
| .................... |
| .................... if (n<0) |
| .................... r = -(float64)-n; |
| .................... else |
| .................... r = (float64)n; |
| .................... |
| .................... res += r*LN2; |
| .................... } |
| .................... |
| .................... else |
| .................... res = 0.0; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... #endif |
| .................... |
| .................... |
| .................... #define LN10 2.3025850929940456 |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float log10(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the the log base 10 of x |
| .................... // Date : N/A |
| .................... // |
| .................... float32 log10(float32 x) |
| .................... { |
| .................... float32 r; |
| .................... |
| .................... r = log(x); |
| .................... r = r/LN10; |
| .................... return(r); |
| .................... } |
| .................... |
| .................... //Overloaded functions for log10() for PCD |
| .................... // Overloaded function log10() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 log10(float48 x) |
| .................... { |
| .................... float48 r; |
| .................... |
| .................... r = log(x); |
| .................... r = r/LN10; |
| .................... return(r); |
| .................... } |
| .................... |
| .................... // Overloaded function log10() for data type - Float64 |
| .................... float64 log10(float64 x) |
| .................... { |
| .................... float64 r; |
| .................... |
| .................... r = log(x); |
| .................... r = r/LN10; |
| .................... return(r); |
| .................... } |
| .................... #endif |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float modf(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description :breaks the argument value int integral and fractional parts, |
| .................... // ach of which have the same sign as the argument. It stores the integral part |
| .................... // as a float in the object pointed to by the iptr |
| .................... // Returns : returns the signed fractional part of value. |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float32 modf(float32 value,float32 *iptr) |
| .................... { |
| .................... *iptr=(value < 0.0)? ceil(value): floor(value); |
| .................... return(value - *iptr); |
| .................... } |
| .................... //Overloaded functions for modf() for PCD |
| .................... // Overloaded function modf() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 modf(float48 value,float48 *iptr) |
| .................... { |
| .................... *iptr=(value < 0.0)? ceil(value): floor(value); |
| .................... return(value - *iptr); |
| .................... } |
| .................... // Overloaded function modf() for data type - Float64 |
| .................... float64 modf(float64 value,float64 *iptr) |
| .................... { |
| .................... *iptr=(value < 0.0)? ceil(value): floor(value); |
| .................... return(value - *iptr); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float pwr(float x,float y) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the value (x^y) |
| .................... // Date : N/A |
| .................... // Note : 0 is returned when the function will generate an imaginary number |
| .................... // |
| .................... float32 pwr(float32 x,float32 y) |
| .................... { |
| .................... if(0 > x && fmod(y, 1) == 0) { |
| .................... if(fmod(y, 2) == 0) { |
| .................... return (exp(log(-x) * y)); |
| .................... } else { |
| .................... return (-exp(log(-x) * y)); |
| .................... } |
| .................... } else if(0 > x && fmod(y, 1) != 0) { |
| .................... return 0; |
| .................... } else { |
| .................... if(x != 0 || 0 >= y) { |
| .................... return (exp(log(x) * y)); |
| .................... } |
| .................... } |
| .................... } |
| .................... //Overloaded functions for pwr() for PCD |
| .................... // Overloaded function pwr() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 pwr(float48 x,float48 y) |
| .................... { |
| .................... if(0 > x && fmod(y, 1) == 0) { |
| .................... if(fmod(y, 2) == 0) { |
| .................... return (exp(log(-x) * y)); |
| .................... } else { |
| .................... return (-exp(log(-x) * y)); |
| .................... } |
| .................... } else if(0 > x && fmod(y, 1) != 0) { |
| .................... return 0; |
| .................... } else { |
| .................... if(x != 0 || 0 >= y) { |
| .................... return (exp(log(x) * y)); |
| .................... } |
| .................... } |
| .................... } |
| .................... // Overloaded function pwr() for data type - Float64 |
| .................... float64 pwr(float64 x,float64 y) |
| .................... { |
| .................... if(0 > x && fmod(y, 1) == 0) { |
| .................... if(fmod(y, 2) == 0) { |
| .................... return (exp(log(-x) * y)); |
| .................... } else { |
| .................... return (-exp(log(-x) * y)); |
| .................... } |
| .................... } else if(0 > x && fmod(y, 1) != 0) { |
| .................... return 0; |
| .................... } else { |
| .................... if(x != 0 || 0 >= y) { |
| .................... return (exp(log(x) * y)); |
| .................... } |
| .................... } |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////// Power functions //////////////////// |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float pow(float x,float y) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the value (x^y) |
| .................... // Date : N/A |
| .................... // Note : 0 is returned when the function will generate an imaginary number |
| .................... // |
| .................... float32 pow(float32 x,float32 y) |
| .................... { |
| .................... if(0 > x && fmod(y, 1) == 0) { |
| .................... if(fmod(y, 2) == 0) { |
| .................... return (exp(log(-x) * y)); |
| .................... } else { |
| .................... return (-exp(log(-x) * y)); |
| .................... } |
| .................... } else if(0 > x && fmod(y, 1) != 0) { |
| .................... return 0; |
| .................... } else { |
| .................... if(x != 0 || 0 >= y) { |
| .................... return (exp(log(x) * y)); |
| .................... } |
| .................... } |
| .................... } |
| .................... //Overloaded functions for pow() for PCD |
| .................... // Overloaded function for pow() data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 pow(float48 x,float48 y) |
| .................... { |
| .................... if(0 > x && fmod(y, 1) == 0) { |
| .................... if(fmod(y, 2) == 0) { |
| .................... return (exp(log(-x) * y)); |
| .................... } else { |
| .................... return (-exp(log(-x) * y)); |
| .................... } |
| .................... } else if(0 > x && fmod(y, 1) != 0) { |
| .................... return 0; |
| .................... } else { |
| .................... if(x != 0 || 0 >= y) { |
| .................... return (exp(log(x) * y)); |
| .................... } |
| .................... } |
| .................... } |
| .................... |
| .................... // Overloaded function pow() for data type - Float64 |
| .................... float64 pow(float64 x,float64 y) |
| .................... { |
| .................... if(0 > x && fmod(y, 1) == 0) { |
| .................... if(fmod(y, 2) == 0) { |
| .................... return (exp(log(-x) * y)); |
| .................... } else { |
| .................... return (-exp(log(-x) * y)); |
| .................... } |
| .................... } else if(0 > x && fmod(y, 1) != 0) { |
| .................... return 0; |
| .................... } else { |
| .................... if(x != 0 || 0 >= y) { |
| .................... return (exp(log(x) * y)); |
| .................... } |
| .................... } |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float sqrt(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the square root of x |
| .................... // Date : N/A |
| .................... // |
| .................... float32 sqrt(float32 x) |
| .................... { |
| .................... float32 y, res; |
| .................... #if defined(__PCD__) |
| .................... unsigned int16 data1,data2; |
| .................... #endif |
| .................... BYTE *p; |
| .................... |
| .................... #ifdef _ERRNO |
| .................... if(x < 0) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... |
| .................... if( x<=0.0) |
| .................... return(0.0); |
| .................... |
| .................... y=x; |
| .................... |
| .................... #if !defined(__PCD__) |
| .................... p=&y; |
| .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); |
| .................... #endif |
| .................... |
| .................... #if defined(__PCD__) |
| .................... p = (((unsigned int8 *)(&y))+3); |
| .................... data1 = *(((unsigned int8 *)(&y))+3); |
| .................... data2 = *(((unsigned int8 *)(&y))+2); |
| .................... rotate_left(&data1,1); |
| .................... if(bit_test(data2,7)) |
| .................... bit_set(data1,0); |
| .................... data1 = ((data1+127) >>1); |
| .................... bit_clear(data2,7); |
| .................... if(bit_test(data1,0)) |
| .................... bit_set(data2,7); |
| .................... data1 = data1 >>1; |
| .................... *(((unsigned int8 *)(&y))+3) = data1; |
| .................... *(((unsigned int8 *)(&y))+2) = data2; |
| .................... |
| .................... #endif |
| .................... |
| .................... do { |
| .................... res=y; |
| .................... y+=(x/y); |
| .................... |
| .................... #if !defined(__PCD__) |
| .................... (*p)--; |
| .................... #endif |
| .................... |
| .................... #if defined(__PCD__) |
| .................... data1 = *(((unsigned int8 *)(&y))+3); |
| .................... data2 = *(((unsigned int8 *)(&y))+2); |
| .................... rotate_left(&data1,1); |
| .................... if(bit_test(data2,7)) |
| .................... bit_set(data1,0); |
| .................... data1--; |
| .................... bit_clear(data2,7); |
| .................... if(bit_test(data1,0)) |
| .................... bit_set(data2,7); |
| .................... data1 = data1 >>1; |
| .................... *(((unsigned int8 *)(&y))+3) = data1; |
| .................... *(((unsigned int8 *)(&y))+2) = data2; |
| .................... |
| .................... #endif |
| .................... } while(res != y); |
| .................... |
| .................... return(res); |
| .................... } |
| .................... //Overloaded functions for sqrt() for PCD |
| .................... // Overloaded function sqrt() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 sqrt(float48 x) |
| .................... { |
| .................... float48 y, res; |
| .................... unsigned int16 data1,data2; |
| .................... BYTE *p; |
| .................... |
| .................... #ifdef _ERRNO |
| .................... if(x < 0) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... |
| .................... if( x<=0.0) |
| .................... return(0.0); |
| .................... |
| .................... y=x; |
| .................... |
| .................... #if !defined(__PCD__) |
| .................... p=&y; |
| .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); |
| .................... #endif |
| .................... |
| .................... #if defined(__PCD__) |
| .................... p = (((unsigned int8 *)(&y))+5); |
| .................... data1 = *(((unsigned int8 *)(&y))+5); |
| .................... data2 = *(((unsigned int8 *)(&y))+4); |
| .................... rotate_left(&data1,1); |
| .................... if(bit_test(data2,7)) |
| .................... bit_set(data1,0); |
| .................... data1 = ((data1+127) >>1); |
| .................... bit_clear(data2,7); |
| .................... if(bit_test(data1,0)) |
| .................... bit_set(data2,7); |
| .................... data1 = data1 >>1; |
| .................... *(((unsigned int8 *)(&y))+5) = data1; |
| .................... *(((unsigned int8 *)(&y))+4) = data2; |
| .................... |
| .................... #endif |
| .................... |
| .................... do { |
| .................... res=y; |
| .................... y+=(x/y); |
| .................... |
| .................... #if !defined(__PCD__) |
| .................... (*p)--; |
| .................... #endif |
| .................... |
| .................... data1 = *(((unsigned int8 *)(&y))+5); |
| .................... data2 = *(((unsigned int8 *)(&y))+4); |
| .................... rotate_left(&data1,1); |
| .................... if(bit_test(data2,7)) |
| .................... bit_set(data1,0); |
| .................... data1--; |
| .................... bit_clear(data2,7); |
| .................... if(bit_test(data1,0)) |
| .................... bit_set(data2,7); |
| .................... data1 = data1 >>1; |
| .................... *(((unsigned int8 *)(&y))+5) = data1; |
| .................... *(((unsigned int8 *)(&y))+4) = data2; |
| .................... |
| .................... } while(res != y); |
| .................... |
| .................... return(res); |
| .................... } |
| .................... |
| .................... // Overloaded function sqrt() for data type - Float64 |
| .................... float64 sqrt(float64 x) |
| .................... { |
| .................... float64 y, res; |
| .................... unsigned int16 *p; |
| .................... unsigned int16 temp1,temp2; |
| .................... |
| .................... #ifdef _ERRNO |
| .................... if(x < 0) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... |
| .................... if( x<=0.0) |
| .................... return(0.0); |
| .................... |
| .................... y=x; |
| .................... p= (((unsigned int16 *)(&y))+3); |
| .................... temp1 = *p; |
| .................... temp2 = *p; |
| .................... bit_clear(temp1,15); |
| .................... temp1 = (temp1>>4)+1023; |
| .................... temp1 = temp1 >> 1; |
| .................... temp1 = (temp1<<4) & 0xFFF0; |
| .................... if(bit_test(temp2,15)) |
| .................... bit_set(temp1,15); |
| .................... temp2 = temp2 & 0x000F; |
| .................... temp1 ^= temp2; |
| .................... |
| .................... (*p) = temp1; |
| .................... |
| .................... do { |
| .................... res=y; |
| .................... y+=(x/y); |
| .................... temp1 = *p; |
| .................... temp2 = *p; |
| .................... bit_clear(temp1,15); |
| .................... temp1 = (temp1>>4); |
| .................... temp1--; |
| .................... temp1 = (temp1<<4) & 0xFFF0; |
| .................... if(bit_test(temp2,15)) |
| .................... bit_set(temp1,15); |
| .................... temp2 = temp2 & 0x000F; |
| .................... temp1 ^= temp2; |
| .................... (*p) = temp1; |
| .................... |
| .................... } while(res != y); |
| .................... |
| .................... return(res); |
| .................... } |
| .................... #endif |
| .................... |
| .................... ////////////////////////////// Trig Functions ////////////////////////////// |
| .................... #ifdef PI_DIV_BY_TWO |
| .................... #undef PI_DIV_BY_TWO |
| .................... #endif |
| .................... #define PI_DIV_BY_TWO 1.5707963267948966 |
| .................... #ifdef TWOBYPI |
| .................... #undef TWOBYPI |
| .................... #define TWOBYPI 0.6366197723675813 |
| .................... #endif |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float cos(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the cosine value of the angle x, which is in radian |
| .................... // Date : 9/20/2001 |
| .................... // |
| .................... float32 cos(float32 x) |
| .................... { |
| .................... float32 y, t, t2 = 1.0; |
| .................... unsigned int8 quad, i; |
| .................... float32 frac; |
| .................... float32 p[6] = { //by the series definition for cosine |
| .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) |
| .................... 0.04166666666667, |
| .................... -0.00138888888889, |
| .................... 0.00002480158730, |
| .................... -0.00000027557319, |
| .................... 0.00000000208767, |
| .................... //-0.00000000001147, |
| .................... // 0.00000000000005 |
| .................... }; |
| .................... |
| .................... if (x < 0) x = -x; // absolute value of input |
| .................... |
| .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant |
| .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input |
| .................... quad = quad % 4; // quadrant (0 to 3) |
| .................... |
| .................... if (quad == 0 || quad == 2) |
| .................... t = frac * PI_DIV_BY_TWO; |
| .................... else if (quad == 1) |
| .................... t = (1-frac) * PI_DIV_BY_TWO; |
| .................... else // should be 3 |
| .................... t = (frac-1) * PI_DIV_BY_TWO; |
| .................... |
| .................... y = 1.0; |
| .................... t = t * t; |
| .................... for (i = 0; i <= 5; i++) |
| .................... { |
| .................... t2 = t2 * t; |
| .................... y = y + p[i] * t2; |
| .................... } |
| .................... |
| .................... if (quad == 2 || quad == 1) |
| .................... y = -y; // correct sign |
| .................... |
| .................... return (y); |
| .................... } |
| .................... |
| .................... |
| .................... //Overloaded functions for cos() for PCD |
| .................... // Overloaded function cos() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 cos(float48 x) |
| .................... { |
| .................... float48 y, t, t2 = 1.0; |
| .................... unsigned int8 quad, i; |
| .................... float48 frac; |
| .................... float48 p[6] = { //by the series definition for cosine |
| .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) |
| .................... 0.04166666666667, |
| .................... -0.00138888888889, |
| .................... 0.00002480158730, |
| .................... -0.00000027557319, |
| .................... 0.00000000208767, |
| .................... //-0.00000000001147, |
| .................... // 0.00000000000005 |
| .................... }; |
| .................... |
| .................... if (x < 0) x = -x; // absolute value of input |
| .................... |
| .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant |
| .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input |
| .................... quad = quad % 4; // quadrant (0 to 3) |
| .................... |
| .................... if (quad == 0 || quad == 2) |
| .................... t = frac * PI_DIV_BY_TWO; |
| .................... else if (quad == 1) |
| .................... t = (1-frac) * PI_DIV_BY_TWO; |
| .................... else // should be 3 |
| .................... t = (frac-1) * PI_DIV_BY_TWO; |
| .................... |
| .................... y = 0.999999999781; |
| .................... t = t * t; |
| .................... for (i = 0; i <= 5; i++) |
| .................... { |
| .................... t2 = t2 * t; |
| .................... y = y + p[i] * t2; |
| .................... } |
| .................... |
| .................... if (quad == 2 || quad == 1) |
| .................... y = -y; // correct sign |
| .................... |
| .................... return (y); |
| .................... } |
| .................... |
| .................... // Overloaded function cos() for data type - Float48 |
| .................... float64 cos(float64 x) |
| .................... { |
| .................... float64 y, t, t2 = 1.0; |
| .................... unsigned int8 quad, i; |
| .................... float64 frac; |
| .................... float64 p[6] = { //by the series definition for cosine |
| .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) |
| .................... 0.04166666666667, |
| .................... -0.00138888888889, |
| .................... 0.00002480158730, |
| .................... -0.00000027557319, |
| .................... 0.00000000208767, |
| .................... //-0.00000000001147, |
| .................... // 0.00000000000005 |
| .................... }; |
| .................... |
| .................... if (x < 0) x = -x; // absolute value of input |
| .................... |
| .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant |
| .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input |
| .................... quad = quad % 4; // quadrant (0 to 3) |
| .................... |
| .................... if (quad == 0 || quad == 2) |
| .................... t = frac * PI_DIV_BY_TWO; |
| .................... else if (quad == 1) |
| .................... t = (1-frac) * PI_DIV_BY_TWO; |
| .................... else // should be 3 |
| .................... t = (frac-1) * PI_DIV_BY_TWO; |
| .................... |
| .................... y = 0.999999999781; |
| .................... t = t * t; |
| .................... for (i = 0; i <= 5; i++) |
| .................... { |
| .................... t2 = t2 * t; |
| .................... y = y + p[i] * t2; |
| .................... } |
| .................... |
| .................... if (quad == 2 || quad == 1) |
| .................... y = -y; // correct sign |
| .................... |
| .................... return (y); |
| .................... } |
| .................... |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float sin(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the sine value of the angle x, which is in radian |
| .................... // Date : 9/20/2001 |
| .................... // |
| .................... float32 sin(float32 x) |
| .................... { |
| .................... return cos(x - PI_DIV_BY_TWO); |
| .................... } |
| .................... |
| .................... //Overloaded functions for sin() for PCD |
| .................... // Overloaded function sin() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 sin(float48 x) |
| .................... { |
| .................... return cos(x - PI_DIV_BY_TWO); |
| .................... } |
| .................... |
| .................... // Overloaded function sin() for data type - Float48 |
| .................... float64 sin(float64 x) |
| .................... { |
| .................... return cos(x - PI_DIV_BY_TWO); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float tan(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the tangent value of the angle x, which is in radian |
| .................... // Date : 9/20/2001 |
| .................... // |
| .................... float32 tan(float32 x) |
| .................... { |
| .................... float32 c, s; |
| .................... |
| .................... c = cos(x); |
| .................... if (c == 0.0) |
| .................... return (1.0e+36); |
| .................... |
| .................... s = sin(x); |
| .................... return(s/c); |
| .................... } |
| .................... //Overloaded functions for tan() for PCD |
| .................... // Overloaded function tan() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 tan(float48 x) |
| .................... { |
| .................... float48 c, s; |
| .................... |
| .................... c = cos(x); |
| .................... if (c == 0.0) |
| .................... return (1.0e+36); |
| .................... |
| .................... s = sin(x); |
| .................... return(s/c); |
| .................... } |
| .................... |
| .................... // Overloaded function tan() for data type - Float48 |
| .................... float64 tan(float64 x) |
| .................... { |
| .................... float64 c, s; |
| .................... |
| .................... c = cos(x); |
| .................... if (c == 0.0) |
| .................... return (1.0e+36); |
| .................... |
| .................... s = sin(x); |
| .................... return(s/c); |
| .................... } |
| .................... #endif |
| .................... |
| .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290}; |
| .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290}; |
| .................... |
| .................... float32 ASIN_COS(float32 x, unsigned int8 n) |
| .................... { |
| .................... float32 y, res, r, y2; |
| .................... int1 s; |
| .................... #ifdef _ERRNO |
| .................... if(x <-1 || x > 1) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y > 0.5) |
| .................... { |
| .................... y = sqrt((1.0 - y)/2.0); |
| .................... n += 2; |
| .................... } |
| .................... |
| .................... y2=y*y; |
| .................... |
| .................... res = pas[0]*y2 + pas[1]; |
| .................... res = res*y2 + pas[2]; |
| .................... |
| .................... r = qas[0]*y2 + qas[1]; |
| .................... r = r*y2 + qas[2]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... if (n & 2) // |x| > 0.5 |
| .................... res = PI_DIV_BY_TWO - 2.0*res; |
| .................... if (s) |
| .................... res = -res; |
| .................... if (n & 1) // take arccos |
| .................... res = PI_DIV_BY_TWO - res; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... |
| .................... //Overloaded functions for ASIN_COS() for PCD |
| .................... // Overloaded function ASIN_COS() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 ASIN_COS(float48 x, unsigned int8 n) |
| .................... { |
| .................... float48 y, res, r, y2; |
| .................... int1 s; |
| .................... #ifdef _ERRNO |
| .................... if(x <-1 || x > 1) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y > 0.5) |
| .................... { |
| .................... y = sqrt((1.0 - y)/2.0); |
| .................... n += 2; |
| .................... } |
| .................... |
| .................... y2=y*y; |
| .................... |
| .................... res = pas[0]*y2 + pas[1]; |
| .................... res = res*y2 + pas[2]; |
| .................... |
| .................... r = qas[0]*y2 + qas[1]; |
| .................... r = r*y2 + qas[2]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... if (n & 2) // |x| > 0.5 |
| .................... res = PI_DIV_BY_TWO - 2.0*res; |
| .................... if (s) |
| .................... res = -res; |
| .................... if (n & 1) // take arccos |
| .................... res = PI_DIV_BY_TWO - res; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... |
| .................... // Overloaded function ASIN_COS() for data type - Float64 |
| .................... float64 ASIN_COS(float64 x, unsigned int8 n) |
| .................... { |
| .................... float64 y, res, r, y2; |
| .................... int1 s; |
| .................... #ifdef _ERRNO |
| .................... if(x <-1 || x > 1) |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... s = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y > 0.5) |
| .................... { |
| .................... y = sqrt((1.0 - y)/2.0); |
| .................... n += 2; |
| .................... } |
| .................... |
| .................... y2=y*y; |
| .................... |
| .................... res = pas[0]*y2 + pas[1]; |
| .................... res = res*y2 + pas[2]; |
| .................... |
| .................... r = qas[0]*y2 + qas[1]; |
| .................... r = r*y2 + qas[2]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... if (n & 2) // |x| > 0.5 |
| .................... res = PI_DIV_BY_TWO - 2.0*res; |
| .................... if (s) |
| .................... res = -res; |
| .................... if (n & 1) // take arccos |
| .................... res = PI_DIV_BY_TWO - res; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float asin(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the arcsine value of the value x. |
| .................... // Date : N/A |
| .................... // |
| .................... float32 asin(float32 x) |
| .................... { |
| .................... float32 r; |
| .................... |
| .................... r = ASIN_COS(x, 0); |
| .................... return(r); |
| .................... } |
| .................... //Overloaded functions for asin() for PCD |
| .................... // Overloaded function asin() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 asin(float48 x) |
| .................... { |
| .................... float48 r; |
| .................... |
| .................... r = ASIN_COS(x, 0); |
| .................... return(r); |
| .................... } |
| .................... |
| .................... // Overloaded function asin() for data type - Float64 |
| .................... float64 asin(float64 x) |
| .................... { |
| .................... float64 r; |
| .................... |
| .................... r = ASIN_COS(x, 0); |
| .................... return(r); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float acos(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the arccosine value of the value x. |
| .................... // Date : N/A |
| .................... // |
| .................... float32 acos(float32 x) |
| .................... { |
| .................... float32 r; |
| .................... |
| .................... r = ASIN_COS(x, 1); |
| .................... return(r); |
| .................... } |
| .................... //Overloaded functions for acos() for PCD |
| .................... // Overloaded function acos() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 acos(float48 x) |
| .................... { |
| .................... float48 r; |
| .................... |
| .................... r = ASIN_COS(x, 1); |
| .................... return(r); |
| .................... } |
| .................... |
| .................... // Overloaded function acos() for data type - Float64 |
| .................... float64 acos(float64 x) |
| .................... { |
| .................... float64 r; |
| .................... |
| .................... r = ASIN_COS(x, 1); |
| .................... return(r); |
| .................... } |
| .................... #endif |
| .................... |
| .................... float32 const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; |
| .................... float32 const qat[4] = {1.0000000, 11.368190, 28.982246, 19.818457}; |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float atan(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : returns the arctangent value of the value x. |
| .................... // Date : N/A |
| .................... // |
| .................... float32 atan(float32 x) |
| .................... { |
| .................... float32 y, res, r; |
| .................... int1 s, flag; |
| .................... |
| .................... s = 0; |
| .................... flag = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y > 1.0) |
| .................... { |
| .................... y = 1.0/y; |
| .................... flag = 1; |
| .................... } |
| .................... |
| .................... res = pat[0]*y*y + pat[1]; |
| .................... res = res*y*y + pat[2]; |
| .................... res = res*y*y + pat[3]; |
| .................... |
| .................... r = qat[0]*y*y + qat[1]; |
| .................... r = r*y*y + qat[2]; |
| .................... r = r*y*y + qat[3]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... |
| .................... if (flag) // for |x| > 1 |
| .................... res = PI_DIV_BY_TWO - res; |
| .................... if (s) |
| .................... res = -res; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... //Overloaded functions for atan() for PCD |
| .................... // Overloaded function atan() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 atan(float48 x) |
| .................... { |
| .................... float48 y, res, r; |
| .................... int1 s, flag; |
| .................... |
| .................... s = 0; |
| .................... flag = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y > 1.0) |
| .................... { |
| .................... y = 1.0/y; |
| .................... flag = 1; |
| .................... } |
| .................... |
| .................... res = pat[0]*y*y + pat[1]; |
| .................... res = res*y*y + pat[2]; |
| .................... res = res*y*y + pat[3]; |
| .................... |
| .................... r = qat[0]*y*y + qat[1]; |
| .................... r = r*y*y + qat[2]; |
| .................... r = r*y*y + qat[3]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... |
| .................... if (flag) // for |x| > 1 |
| .................... res = PI_DIV_BY_TWO - res; |
| .................... if (s) |
| .................... res = -res; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... |
| .................... // Overloaded function atan() for data type - Float64 |
| .................... float64 atan(float64 x) |
| .................... { |
| .................... float64 y, res, r; |
| .................... int1 s, flag; |
| .................... |
| .................... s = 0; |
| .................... flag = 0; |
| .................... y = x; |
| .................... |
| .................... if (x < 0) |
| .................... { |
| .................... s = 1; |
| .................... y = -y; |
| .................... } |
| .................... |
| .................... if (y > 1.0) |
| .................... { |
| .................... y = 1.0/y; |
| .................... flag = 1; |
| .................... } |
| .................... |
| .................... res = pat[0]*y*y + pat[1]; |
| .................... res = res*y*y + pat[2]; |
| .................... res = res*y*y + pat[3]; |
| .................... |
| .................... r = qat[0]*y*y + qat[1]; |
| .................... r = r*y*y + qat[2]; |
| .................... r = r*y*y + qat[3]; |
| .................... |
| .................... res = y*res/r; |
| .................... |
| .................... |
| .................... if (flag) // for |x| > 1 |
| .................... res = PI_DIV_BY_TWO - res; |
| .................... if (s) |
| .................... res = -res; |
| .................... |
| .................... return(res); |
| .................... } |
| .................... #endif |
| .................... |
| .................... ///////////////////////////////////////////////////////////////////////////// |
| .................... // float atan2(float y, float x) |
| .................... ///////////////////////////////////////////////////////////////////////////// |
| .................... // Description :computes the principal value of arc tangent of y/x, using the |
| .................... // signs of both the arguments to determine the quadrant of the return value |
| .................... // Returns : returns the arc tangent of y/x. |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float32 atan2(float32 y,float32 x) |
| .................... { |
| .................... float32 z; |
| .................... int1 sign; |
| .................... unsigned int8 quad; |
| .................... sign=0; |
| .................... quad=0; //quadrant |
| .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); |
| .................... if(y<0.0) |
| .................... { |
| .................... sign=1; |
| .................... y=-y; |
| .................... } |
| .................... if(x<0.0) |
| .................... { |
| .................... x=-x; |
| .................... } |
| .................... if (x==0.0) |
| .................... { |
| .................... if(y==0.0) |
| .................... { |
| .................... #ifdef _ERRNO |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... } |
| .................... else |
| .................... { |
| .................... if(sign) |
| .................... { |
| .................... return (-(PI_DIV_BY_TWO)); |
| .................... } |
| .................... else |
| .................... { |
| .................... return (PI_DIV_BY_TWO); |
| .................... } |
| .................... } |
| .................... } |
| .................... else |
| .................... { |
| .................... z=y/x; |
| .................... switch(quad) |
| .................... { |
| .................... case 1: |
| .................... { |
| .................... return atan(z); |
| .................... break; |
| .................... } |
| .................... case 2: |
| .................... { |
| .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 |
| .................... return (PI-atan(z)); |
| .................... break; |
| .................... } |
| .................... case 3: |
| .................... { |
| .................... return (atan(z)-PI); |
| .................... break; |
| .................... } |
| .................... case 4: |
| .................... { |
| .................... return (-atan(z)); |
| .................... break; |
| .................... } |
| .................... } |
| .................... } |
| .................... } |
| .................... |
| .................... //Overloaded functions for atan2() for PCD |
| .................... // Overloaded function atan2() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 atan2(float48 y,float48 x) |
| .................... { |
| .................... float48 z; |
| .................... int1 sign; |
| .................... unsigned int8 quad; |
| .................... sign=0; |
| .................... quad=0; //quadrant |
| .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); |
| .................... if(y<0.0) |
| .................... { |
| .................... sign=1; |
| .................... y=-y; |
| .................... } |
| .................... if(x<0.0) |
| .................... { |
| .................... x=-x; |
| .................... } |
| .................... if (x==0.0) |
| .................... { |
| .................... if(y==0.0) |
| .................... { |
| .................... #ifdef _ERRNO |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... } |
| .................... else |
| .................... { |
| .................... if(sign) |
| .................... { |
| .................... return (-(PI_DIV_BY_TWO)); |
| .................... } |
| .................... else |
| .................... { |
| .................... return (PI_DIV_BY_TWO); |
| .................... } |
| .................... } |
| .................... } |
| .................... else |
| .................... { |
| .................... z=y/x; |
| .................... switch(quad) |
| .................... { |
| .................... case 1: |
| .................... { |
| .................... return atan(z); |
| .................... break; |
| .................... } |
| .................... case 2: |
| .................... { |
| .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 |
| .................... return (PI-atan(z)); |
| .................... break; |
| .................... } |
| .................... case 3: |
| .................... { |
| .................... return (atan(z)-PI); |
| .................... break; |
| .................... } |
| .................... case 4: |
| .................... { |
| .................... return (-atan(z)); |
| .................... break; |
| .................... } |
| .................... } |
| .................... } |
| .................... } |
| .................... |
| .................... // Overloaded function atan2() for data type - Float64 |
| .................... float64 atan2(float64 y,float64 x) |
| .................... { |
| .................... float64 z; |
| .................... int1 sign; |
| .................... unsigned int8 quad; |
| .................... sign=0; |
| .................... quad=0; //quadrant |
| .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); |
| .................... if(y<0.0) |
| .................... { |
| .................... sign=1; |
| .................... y=-y; |
| .................... } |
| .................... if(x<0.0) |
| .................... { |
| .................... x=-x; |
| .................... } |
| .................... if (x==0.0) |
| .................... { |
| .................... if(y==0.0) |
| .................... { |
| .................... #ifdef _ERRNO |
| .................... { |
| .................... errno=EDOM; |
| .................... } |
| .................... #endif |
| .................... } |
| .................... else |
| .................... { |
| .................... if(sign) |
| .................... { |
| .................... return (-(PI_DIV_BY_TWO)); |
| .................... } |
| .................... else |
| .................... { |
| .................... return (PI_DIV_BY_TWO); |
| .................... } |
| .................... } |
| .................... } |
| .................... else |
| .................... { |
| .................... z=y/x; |
| .................... switch(quad) |
| .................... { |
| .................... case 1: |
| .................... { |
| .................... return atan(z); |
| .................... break; |
| .................... } |
| .................... case 2: |
| .................... { |
| .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 |
| .................... return (PI-atan(z)); |
| .................... break; |
| .................... } |
| .................... case 3: |
| .................... { |
| .................... return (atan(z)-PI); |
| .................... break; |
| .................... } |
| .................... case 4: |
| .................... { |
| .................... return (-atan(z)); |
| .................... break; |
| .................... } |
| .................... } |
| .................... } |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////// Hyperbolic functions //////////////////// |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float cosh(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : Computes the hyperbolic cosine value of x |
| .................... // Returns : returns the hyperbolic cosine value of x |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float32 cosh(float32 x) |
| .................... { |
| .................... return ((exp(x)+exp(-x))/2); |
| .................... } |
| .................... //Overloaded functions for cosh() for PCD |
| .................... // Overloaded function cosh() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 cosh(float48 x) |
| .................... { |
| .................... return ((exp(x)+exp(-x))/2); |
| .................... } |
| .................... |
| .................... // Overloaded function cosh() for data type - Float64 |
| .................... float64 cosh(float64 x) |
| .................... { |
| .................... return ((exp(x)+exp(-x))/2); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float sinh(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : Computes the hyperbolic sine value of x |
| .................... // Returns : returns the hyperbolic sine value of x |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float32 sinh(float32 x) |
| .................... { |
| .................... |
| .................... return ((exp(x) - exp(-x))/2); |
| .................... } |
| .................... //Overloaded functions for sinh() for PCD |
| .................... // Overloaded function sinh() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 sinh(float48 x) |
| .................... { |
| .................... |
| .................... return ((exp(x) - exp(-x))/2); |
| .................... } |
| .................... |
| .................... // Overloaded function sinh() for data type - Float48 |
| .................... float64 sinh(float64 x) |
| .................... { |
| .................... |
| .................... return ((exp(x) - exp(-x))/2); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float tanh(float x) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : Computes the hyperbolic tangent value of x |
| .................... // Returns : returns the hyperbolic tangent value of x |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float32 tanh(float32 x) |
| .................... { |
| .................... return(sinh(x)/cosh(x)); |
| .................... } |
| .................... //Overloaded functions for tanh() for PCD |
| .................... // Overloaded function tanh() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 tanh(float48 x) |
| .................... { |
| .................... return(sinh(x)/cosh(x)); |
| .................... } |
| .................... |
| .................... // Overloaded function tanh() for data type - Float64 |
| .................... float64 tanh(float64 x) |
| .................... { |
| .................... return(sinh(x)/cosh(x)); |
| .................... } |
| .................... #endif |
| .................... |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // float frexp(float x, signed int *exp) |
| .................... //////////////////////////////////////////////////////////////////////////// |
| .................... // Description : breaks a floating point number into a normalized fraction and an integral |
| .................... // power of 2. It stores the integer in the signed int object pointed to by exp. |
| .................... // Returns : returns the value x, such that x is a double with magnitude in the interval |
| .................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero, |
| .................... // both parts of the result are zero. |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... #define LOG2 .30102999566398119521 |
| .................... float32 frexp(float32 x, signed int8 *exp) |
| .................... { |
| .................... float32 res; |
| .................... int1 sign = 0; |
| .................... if(x == 0.0) |
| .................... { |
| .................... *exp=0; |
| .................... return (0.0); |
| .................... } |
| .................... if(x < 0.0) |
| .................... { |
| .................... x=-x; |
| .................... sign=1; |
| .................... } |
| .................... if (x > 1.0) |
| .................... { |
| .................... *exp=(ceil(log10(x)/LOG2)); |
| .................... res=x/(pow(2, *exp)); |
| .................... if (res == 1) |
| .................... { |
| .................... *exp=*exp+1; |
| .................... res=.5; |
| .................... } |
| .................... } |
| .................... else |
| .................... { |
| .................... if(x < 0.5) |
| .................... { |
| .................... *exp=-1; |
| .................... res=x*2; |
| .................... } |
| .................... else |
| .................... { |
| .................... *exp=0; |
| .................... res=x; |
| .................... } |
| .................... } |
| .................... if(sign) |
| .................... { |
| .................... res=-res; |
| .................... } |
| .................... return res; |
| .................... } |
| .................... |
| .................... //Overloaded functions for frexp() for PCD |
| .................... // Overloaded function frexp() for data type - Float48 |
| .................... #if defined(__PCD__) |
| .................... float48 frexp(float48 x, signed int8 *exp) |
| .................... { |
| .................... float48 res; |
| .................... int1 sign = 0; |
| .................... if(x == 0.0) |
| .................... { |
| .................... *exp=0; |
| .................... return (0.0); |
| .................... } |
| .................... if(x < 0.0) |
| .................... { |
| .................... x=-x; |
| .................... sign=1; |
| .................... } |
| .................... if (x > 1.0) |
| .................... { |
| .................... *exp=(ceil(log10(x)/LOG2)); |
| .................... res=x/(pow(2, *exp)); |
| .................... if (res == 1) |
| .................... { |
| .................... *exp=*exp+1; |
| .................... res=.5; |
| .................... } |
| .................... } |
| .................... else |
| .................... { |
| .................... if(x < 0.5) |
| .................... { |
| .................... *exp=-1; |
| .................... res=x*2; |
| .................... } |
| .................... else |
| .................... { |
| .................... *exp=0; |
| .................... res=x; |
| .................... } |
| .................... } |
| .................... if(sign) |
| .................... { |
| .................... res=-res; |
| .................... } |
| .................... return res; |
| .................... } |
| .................... |
| .................... // Overloaded function frexp() for data type - Float64 |
| .................... float64 frexp(float64 x, signed int8 *exp) |
| .................... { |
| .................... float64 res; |
| .................... int1 sign = 0; |
| .................... if(x == 0.0) |
| .................... { |
| .................... *exp=0; |
| .................... return (0.0); |
| .................... } |
| .................... if(x < 0.0) |
| .................... { |
| .................... x=-x; |
| .................... sign=1; |
| .................... } |
| .................... if (x > 1.0) |
| .................... { |
| .................... *exp=(ceil(log10(x)/LOG2)); |
| .................... res=x/(pow(2, *exp)); |
| .................... if (res == 1) |
| .................... { |
| .................... *exp=*exp+1; |
| .................... res=.5; |
| .................... } |
| .................... } |
| .................... else |
| .................... { |
| .................... if(x < 0.5) |
| .................... { |
| .................... *exp=-1; |
| .................... res=x*2; |
| .................... } |
| .................... else |
| .................... { |
| .................... *exp=0; |
| .................... res=x; |
| .................... } |
| .................... } |
| .................... if(sign) |
| .................... { |
| .................... res=-res; |
| .................... } |
| .................... return res; |
| .................... } |
| .................... #endif |
| .................... |
| .................... ////////////////////////////////////////////////////////////////////////////// |
| .................... // float ldexp(float x, signed int *exp) |
| .................... ////////////////////////////////////////////////////////////////////////////// |
| .................... // Description : multiplies a floating point number by an integral power of 2. |
| .................... // Returns : returns the value of x times 2 raised to the power exp. |
| .................... // Date : N/A |
| .................... // |
| .................... |
| .................... float32 ldexp(float32 value, signed int8 exp) |
| .................... { |
| .................... return (value * pow(2,exp)); |
| .................... } |
| .................... //Overloaded functions for ldexp() for PCD |
| .................... // Overloaded function ldexp() for data type - Float48 |
| .................... |
| .................... #if defined(__PCD__) |
| .................... float48 ldexp(float48 value, signed int8 exp) |
| .................... { |
| .................... return (value * pow(2,exp)); |
| .................... } |
| .................... // Overloaded function ldexp() for data type - Float64 |
| .................... float64 ldexp(float64 value, signed int8 exp) |
| .................... { |
| .................... return (value * pow(2,exp)); |
| .................... } |
| .................... #endif |
| .................... |
| .................... #endif |
| .................... |
| .................... |
| .................... void main() |
| .................... { |
| * |
| 0250: CLRF 04 |
| 0251: BCF 03.7 |
| 0252: MOVLW 1F |
| 0253: ANDWF 03,F |
| 0254: MOVLW 71 |
| 0255: BSF 03.5 |
| 0256: MOVWF 0F |
| 0257: MOVF 0F,W |
| 0258: BCF 03.5 |
| 0259: BCF 20.7 |
| 025A: MOVF 20,W |
| 025B: BSF 03.5 |
| 025C: MOVWF 07 |
| 025D: BCF 03.5 |
| 025E: BSF 07.7 |
| 025F: BSF 03.5 |
| 0260: BSF 03.6 |
| 0261: MOVF 09,W |
| 0262: ANDLW C0 |
| 0263: MOVWF 09 |
| 0264: BCF 03.6 |
| 0265: BCF 1F.4 |
| 0266: BCF 1F.5 |
| 0267: MOVLW 00 |
| 0268: BSF 03.6 |
| 0269: MOVWF 08 |
| 026A: BCF 03.5 |
| 026B: CLRF 07 |
| 026C: CLRF 08 |
| 026D: CLRF 09 |
| 024F: CLRF 04 |
| 0250: BCF 03.7 |
| 0251: MOVLW 1F |
| 0252: ANDWF 03,F |
| 0253: MOVLW 71 |
| 0254: BSF 03.5 |
| 0255: MOVWF 0F |
| 0256: MOVF 0F,W |
| 0257: BCF 03.5 |
| 0258: BCF 20.7 |
| 0259: MOVF 20,W |
| 025A: BSF 03.5 |
| 025B: MOVWF 07 |
| 025C: BCF 03.5 |
| 025D: BSF 07.7 |
| 025E: BSF 03.5 |
| 025F: BSF 03.6 |
| 0260: MOVF 09,W |
| 0261: ANDLW C0 |
| 0262: MOVWF 09 |
| 0263: BCF 03.6 |
| 0264: BCF 1F.4 |
| 0265: BCF 1F.5 |
| 0266: MOVLW 00 |
| 0267: BSF 03.6 |
| 0268: MOVWF 08 |
| 0269: BCF 03.5 |
| 026A: CLRF 07 |
| 026B: CLRF 08 |
| 026C: CLRF 09 |
| * |
| 0277: CLRF 29 |
| 0278: CLRF 28 |
| .................... int1 last; |
| 0276: CLRF 34 |
| 0277: CLRF 33 |
| .................... float last,b,anemo_speed; |
| .................... unsigned int16 anemo_round=0; |
| .................... unsigned int16 i; |
| .................... |
| 565,102 → 2737,102 |
|---|
| .................... |
| .................... //signed int16 X,Y,Z; |
| .................... setup_adc_ports(NO_ANALOGS|VSS_VDD); |
| 0279: BSF 03.5 |
| 027A: BSF 03.6 |
| 027B: MOVF 09,W |
| 027C: ANDLW C0 |
| 027D: MOVWF 09 |
| 027E: BCF 03.6 |
| 027F: BCF 1F.4 |
| 0280: BCF 1F.5 |
| 0281: MOVLW 00 |
| 0282: BSF 03.6 |
| 0283: MOVWF 08 |
| 0278: BSF 03.5 |
| 0279: BSF 03.6 |
| 027A: MOVF 09,W |
| 027B: ANDLW C0 |
| 027C: MOVWF 09 |
| 027D: BCF 03.6 |
| 027E: BCF 1F.4 |
| 027F: BCF 1F.5 |
| 0280: MOVLW 00 |
| 0281: BSF 03.6 |
| 0282: MOVWF 08 |
| .................... setup_adc(ADC_CLOCK_DIV_2); |
| 0284: BCF 03.5 |
| 0285: BCF 03.6 |
| 0286: BCF 1F.6 |
| 0287: BCF 1F.7 |
| 0288: BSF 03.5 |
| 0289: BCF 1F.7 |
| 028A: BCF 03.5 |
| 028B: BSF 1F.0 |
| 0283: BCF 03.5 |
| 0284: BCF 03.6 |
| 0285: BCF 1F.6 |
| 0286: BCF 1F.7 |
| 0287: BSF 03.5 |
| 0288: BCF 1F.7 |
| 0289: BCF 03.5 |
| 028A: BSF 1F.0 |
| .................... setup_spi(SPI_SS_DISABLED); |
| 028C: BCF 14.5 |
| 028D: BCF 20.5 |
| 028E: MOVF 20,W |
| 028F: BSF 03.5 |
| 0290: MOVWF 07 |
| 0291: BCF 03.5 |
| 0292: BSF 20.4 |
| 0293: MOVF 20,W |
| 0294: BSF 03.5 |
| 0295: MOVWF 07 |
| 0296: BCF 03.5 |
| 0297: BCF 20.3 |
| 0298: MOVF 20,W |
| 0299: BSF 03.5 |
| 029A: MOVWF 07 |
| 029B: MOVLW 01 |
| 029C: BCF 03.5 |
| 029D: MOVWF 14 |
| 029E: MOVLW 00 |
| 029F: BSF 03.5 |
| 02A0: MOVWF 14 |
| 028B: BCF 14.5 |
| 028C: BCF 20.5 |
| 028D: MOVF 20,W |
| 028E: BSF 03.5 |
| 028F: MOVWF 07 |
| 0290: BCF 03.5 |
| 0291: BSF 20.4 |
| 0292: MOVF 20,W |
| 0293: BSF 03.5 |
| 0294: MOVWF 07 |
| 0295: BCF 03.5 |
| 0296: BCF 20.3 |
| 0297: MOVF 20,W |
| 0298: BSF 03.5 |
| 0299: MOVWF 07 |
| 029A: MOVLW 01 |
| 029B: BCF 03.5 |
| 029C: MOVWF 14 |
| 029D: MOVLW 00 |
| 029E: BSF 03.5 |
| 029F: MOVWF 14 |
| .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| 02A1: MOVF 01,W |
| 02A2: ANDLW C7 |
| 02A3: IORLW 08 |
| 02A4: MOVWF 01 |
| 02A0: MOVF 01,W |
| 02A1: ANDLW C7 |
| 02A2: IORLW 08 |
| 02A3: MOVWF 01 |
| .................... setup_timer_1(T1_DISABLED); |
| 02A5: BCF 03.5 |
| 02A6: CLRF 10 |
| 02A4: BCF 03.5 |
| 02A5: CLRF 10 |
| .................... setup_timer_2(T2_DISABLED,0,1); |
| 02A7: MOVLW 00 |
| 02A8: MOVWF 78 |
| 02A9: MOVWF 12 |
| 02AA: MOVLW 00 |
| 02AB: BSF 03.5 |
| 02AC: MOVWF 12 |
| 02A6: MOVLW 00 |
| 02A7: MOVWF 78 |
| 02A8: MOVWF 12 |
| 02A9: MOVLW 00 |
| 02AA: BSF 03.5 |
| 02AB: MOVWF 12 |
| .................... setup_ccp1(CCP_OFF); |
| 02AD: BCF 03.5 |
| 02AE: BSF 20.2 |
| 02AF: MOVF 20,W |
| 02B0: BSF 03.5 |
| 02B1: MOVWF 07 |
| 02B2: BCF 03.5 |
| 02B3: CLRF 17 |
| 02B4: BSF 03.5 |
| 02B5: CLRF 1B |
| 02B6: CLRF 1C |
| 02B7: MOVLW 01 |
| 02B8: MOVWF 1D |
| 02AC: BCF 03.5 |
| 02AD: BSF 20.2 |
| 02AE: MOVF 20,W |
| 02AF: BSF 03.5 |
| 02B0: MOVWF 07 |
| 02B1: BCF 03.5 |
| 02B2: CLRF 17 |
| 02B3: BSF 03.5 |
| 02B4: CLRF 1B |
| 02B5: CLRF 1C |
| 02B6: MOVLW 01 |
| 02B7: MOVWF 1D |
| .................... setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard |
| 02B9: BCF 03.5 |
| 02BA: BSF 03.6 |
| 02BB: CLRF 07 |
| 02BC: CLRF 08 |
| 02BD: CLRF 09 |
| 02B8: BCF 03.5 |
| 02B9: BSF 03.6 |
| 02BA: CLRF 07 |
| 02BB: CLRF 08 |
| 02BC: CLRF 09 |
| .................... |
| .................... // Init the HMC5883L. Set Mode register for |
| .................... // continuous measurements. |
| .................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18); // no average, maximal update range |
| 02BE: BCF 03.6 |
| 02BF: CLRF 2C |
| 02C0: MOVLW 18 |
| 02C1: MOVWF 2D |
| 02C2: CALL 069 |
| 02BD: BCF 03.6 |
| 02BE: CLRF 37 |
| 02BF: MOVLW 18 |
| 02C0: MOVWF 38 |
| 02C1: CALL 069 |
| .................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0xE0); // maximal range |
| 02C3: MOVLW 01 |
| 02C4: MOVWF 2C |
| 02C5: MOVLW E0 |
| 02C6: MOVWF 2D |
| 02C7: CALL 069 |
| 02C2: MOVLW 01 |
| 02C3: MOVWF 37 |
| 02C4: MOVLW E0 |
| 02C5: MOVWF 38 |
| 02C6: CALL 069 |
| .................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00); |
| 02C8: MOVLW 02 |
| 02C9: MOVWF 2C |
| 02CA: CLRF 2D |
| 02CB: CALL 069 |
| 02C7: MOVLW 02 |
| 02C8: MOVWF 37 |
| 02C9: CLRF 38 |
| 02CA: CALL 069 |
| .................... |
| .................... // Continuously read and display the x,y,z results. |
| .................... // Wait at least 67 ms between reads, re the HMC5883L data sheet. |
| 667,29 → 2839,29 |
|---|
| .................... |
| .................... |
| .................... printf("Magnetometr: \r\n",); |
| 02CC: MOVLW 04 |
| 02CD: BSF 03.6 |
| 02CE: MOVWF 0D |
| 02CF: MOVLW 00 |
| 02D0: MOVWF 0F |
| 02D1: BCF 03.6 |
| 02D2: CALL 0C6 |
| 02CB: MOVLW 04 |
| 02CC: BSF 03.6 |
| 02CD: MOVWF 0D |
| 02CE: MOVLW 00 |
| 02CF: MOVWF 0F |
| 02D0: BCF 03.6 |
| 02D1: CALL 0C5 |
| .................... printf("(c)mlab JACHO 2013: \r\n",); |
| 02D3: MOVLW 0D |
| 02D4: BSF 03.6 |
| 02D5: MOVWF 0D |
| 02D6: MOVLW 00 |
| 02D7: MOVWF 0F |
| 02D8: BCF 03.6 |
| 02D9: CALL 0C6 |
| 02D2: MOVLW 0D |
| 02D3: BSF 03.6 |
| 02D4: MOVWF 0D |
| 02D5: MOVLW 00 |
| 02D6: MOVWF 0F |
| 02D7: BCF 03.6 |
| 02D8: CALL 0C5 |
| .................... printf("X, Y, Z \r\n",); |
| 02DA: MOVLW 19 |
| 02DB: BSF 03.6 |
| 02DC: MOVWF 0D |
| 02DD: MOVLW 00 |
| 02DE: MOVWF 0F |
| 02DF: BCF 03.6 |
| 02E0: CALL 0C6 |
| 02D9: MOVLW 19 |
| 02DA: BSF 03.6 |
| 02DB: MOVWF 0D |
| 02DC: MOVLW 00 |
| 02DD: MOVWF 0F |
| 02DE: BCF 03.6 |
| 02DF: CALL 0C5 |
| .................... |
| .................... /* while(true) |
| .................... { |
| 705,111 → 2877,67 |
|---|
| .................... while(TRUE) |
| .................... { |
| .................... |
| .................... for( i=0;i<=1000;i++) |
| 02E1: CLRF 2B |
| 02E2: CLRF 2A |
| 02E3: MOVF 2B,W |
| 02E4: SUBLW 03 |
| 02E5: BTFSS 03.0 |
| 02E6: GOTO 303 |
| 02E7: BTFSS 03.2 |
| 02E8: GOTO 2ED |
| 02E9: MOVF 2A,W |
| 02EA: SUBLW E8 |
| 02EB: BTFSS 03.0 |
| 02EC: GOTO 303 |
| .................... { |
| .................... // delay_ms(); |
| .................... hmc5883l_read_data(); |
| 02ED: GOTO 151 |
| .................... // for(i=0;i<=10;i++) |
| .................... // { |
| .................... hmc5883l_read_data(); |
| 02E0: GOTO 150 |
| .................... |
| .................... // b = atan2((float)compass.y,(float)compass.x); // vypocet azimutu z kartezskych souradnic |
| .................... // b = (b/3.141596)*180; // prevod na stupne |
| .................... // b += 180; |
| .................... |
| .................... if(compass.x < 0) |
| 02EE: BTFSS 22.7 |
| 02EF: GOTO 2F6 |
| .................... { |
| .................... if(last == 1) anemo_round++; |
| 02F0: BTFSS 27.0 |
| 02F1: GOTO 2F5 |
| 02F2: INCF 28,F |
| 02F3: BTFSC 03.2 |
| 02F4: INCF 29,F |
| .................... last=0; |
| 02F5: BCF 27.0 |
| .................... } |
| .................... if(compass.x > 0) last=1; |
| 02F6: BTFSC 22.7 |
| 02F7: GOTO 2FF |
| 02F8: MOVF 22,F |
| 02F9: BTFSS 03.2 |
| 02FA: GOTO 2FE |
| 02FB: MOVF 21,W |
| 02FC: SUBLW 00 |
| 02FD: BTFSS 03.0 |
| 02FE: BSF 27.0 |
| .................... } |
| 02FF: INCF 2A,F |
| 0300: BTFSC 03.2 |
| 0301: INCF 2B,F |
| 0302: GOTO 2E3 |
| .................... // anemo_speed += (b-last); |
| .................... // last=b; |
| .................... // delay_ms(10); |
| .................... // } |
| .................... // anemo_speed=anemo_speed/10; |
| .................... |
| .................... printf("%6Ld %6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z, anemo_round); |
| 0303: MOVLW 00 |
| 0304: MOVWF 04 |
| 0305: MOVF 22,W |
| 0306: MOVWF 2D |
| 0307: MOVF 21,W |
| 0308: MOVWF 2C |
| 0309: CALL 1D0 |
| 030A: MOVLW 20 |
| 030B: MOVWF 35 |
| 030C: CALL 0A2 |
| 030D: MOVLW 00 |
| 030E: MOVWF 04 |
| 030F: MOVF 24,W |
| 0310: MOVWF 2D |
| 0311: MOVF 23,W |
| 0312: MOVWF 2C |
| 0313: CALL 1D0 |
| 0314: MOVLW 20 |
| 0315: MOVWF 35 |
| 0316: CALL 0A2 |
| 0317: MOVLW 00 |
| 0318: MOVWF 04 |
| 0319: MOVF 26,W |
| 031A: MOVWF 2D |
| 031B: MOVF 25,W |
| 031C: MOVWF 2C |
| 031D: CALL 1D0 |
| 031E: MOVLW 20 |
| 031F: MOVWF 35 |
| 0320: CALL 0A2 |
| 0321: MOVLW 00 |
| 0322: MOVWF 04 |
| 0323: MOVF 29,W |
| 0324: MOVWF 2D |
| 0325: MOVF 28,W |
| 0326: MOVWF 2C |
| 0327: CALL 1D0 |
| 0328: MOVLW 20 |
| 0329: MOVWF 35 |
| 032A: CALL 0A2 |
| 032B: MOVLW 0A |
| 032C: MOVWF 35 |
| 032D: CALL 0A2 |
| 032E: MOVLW 0D |
| 032F: MOVWF 35 |
| 0330: CALL 0A2 |
| .................... anemo_round=0; |
| 0331: CLRF 29 |
| 0332: CLRF 28 |
| .................... printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z); |
| 02E1: MOVLW 00 |
| 02E2: MOVWF 04 |
| 02E3: MOVF 22,W |
| 02E4: MOVWF 38 |
| 02E5: MOVF 21,W |
| 02E6: MOVWF 37 |
| 02E7: CALL 1CF |
| 02E8: MOVLW 20 |
| 02E9: MOVWF 40 |
| 02EA: CALL 0A2 |
| 02EB: MOVLW 00 |
| 02EC: MOVWF 04 |
| 02ED: MOVF 24,W |
| 02EE: MOVWF 38 |
| 02EF: MOVF 23,W |
| 02F0: MOVWF 37 |
| 02F1: CALL 1CF |
| 02F2: MOVLW 20 |
| 02F3: MOVWF 40 |
| 02F4: CALL 0A2 |
| 02F5: MOVLW 00 |
| 02F6: MOVWF 04 |
| 02F7: MOVF 26,W |
| 02F8: MOVWF 38 |
| 02F9: MOVF 25,W |
| 02FA: MOVWF 37 |
| 02FB: CALL 1CF |
| 02FC: MOVLW 20 |
| 02FD: MOVWF 40 |
| 02FE: CALL 0A2 |
| 02FF: MOVLW 0A |
| 0300: MOVWF 40 |
| 0301: CALL 0A2 |
| 0302: MOVLW 0D |
| 0303: MOVWF 40 |
| 0304: CALL 0A2 |
| .................... // delay_ms(100); |
| .................... } |
| 0333: GOTO 2E1 |
| 0305: GOTO 2E0 |
| .................... |
| .................... } |
| .................... |
| .................... |
| .................... |
| 0334: SLEEP |
| 0306: SLEEP |
| |
| Configuration Fuses: |
| Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG |