Rev 2998 Rev 3275
1 CCS PCM C Compiler, Version 4.106, 47914 17-V-13 12:14 1 CCS PCM C Compiler, Version 4.106, 47914 25-VIII-13 18:55
2   2  
3 Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst 3 Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst
4   4  
5 ROM used: 799 words (10%) 5 ROM used: 762 words (9%)
6 Largest free fragment is 2048 6 Largest free fragment is 2048
7 RAM used: 28 (8%) at main() level 7 RAM used: 12 (3%) at main() level
8 38 (10%) worst case 8 21 (6%) worst case
9 Stack: 2 locations 9 Stack: 2 locations
10   10  
11 * 11 *
12 0000: MOVLW 02 12 0000: MOVLW 02
13 0001: MOVWF 0A 13 0001: MOVWF 0A
14 0002: GOTO 266 14 0002: GOTO 23C
15 0003: NOP 15 0003: NOP
16 .................... #include "main.h" 16 .................... #include "main.h"
17 .................... #include <16F887.h> 17 .................... #include <16F887.h>
18 .................... //////// Standard Header file for the PIC16F887 device //////////////// 18 .................... //////// Standard Header file for the PIC16F887 device ////////////////
19 .................... #device PIC16F887 19 .................... #device PIC16F887
20 .................... #list 20 .................... #list
21 .................... 21 ....................
22 .................... #device adc=8 22 .................... #device adc=8
23 .................... 23 ....................
24 .................... #FUSES NOWDT //No Watch Dog Timer 24 .................... #FUSES NOWDT //No Watch Dog Timer
25 .................... #FUSES INTRC //Internal RC Osc 25 .................... #FUSES INTRC //Internal RC Osc
26 .................... #FUSES NOPUT //No Power Up Timer 26 .................... #FUSES NOPUT //No Power Up Timer
27 .................... #FUSES MCLR //Master Clear pin enabled 27 .................... #FUSES MCLR //Master Clear pin enabled
28 .................... #FUSES NOPROTECT //Code not protected from reading 28 .................... #FUSES NOPROTECT //Code not protected from reading
29 .................... #FUSES NOCPD //No EE protection 29 .................... #FUSES NOCPD //No EE protection
30 .................... #FUSES NOBROWNOUT //No brownout reset 30 .................... #FUSES NOBROWNOUT //No brownout reset
31 .................... #FUSES IESO //Internal External Switch Over mode enabled 31 .................... #FUSES IESO //Internal External Switch Over mode enabled
32 .................... #FUSES FCMEN //Fail-safe clock monitor enabled 32 .................... #FUSES FCMEN //Fail-safe clock monitor enabled
33 .................... #FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O 33 .................... #FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
34 .................... #FUSES NODEBUG //No Debug mode for ICD 34 .................... #FUSES NODEBUG //No Debug mode for ICD
35 .................... #FUSES NOWRT //Program memory not write protected 35 .................... #FUSES NOWRT //Program memory not write protected
36 .................... #FUSES BORV40 //Brownout reset at 4.0V 36 .................... #FUSES BORV40 //Brownout reset at 4.0V
37 .................... 37 ....................
38 .................... #use delay(clock=8000000) 38 .................... #use delay(clock=8000000)
39 * 39 *
40 0250: MOVLW 37 40 0226: MOVLW 27
41 0251: MOVWF 04 41 0227: MOVWF 04
42 0252: BCF 03.7 42 0228: BCF 03.7
43 0253: MOVF 00,W 43 0229: MOVF 00,W
44 0254: BTFSC 03.2 44 022A: BTFSC 03.2
45 0255: GOTO 263 45 022B: GOTO 239
46 0256: MOVLW 02 46 022C: MOVLW 02
47 0257: MOVWF 78 47 022D: MOVWF 78
48 0258: CLRF 77 48 022E: CLRF 77
49 0259: DECFSZ 77,F 49 022F: DECFSZ 77,F
50 025A: GOTO 259 50 0230: GOTO 22F
51 025B: DECFSZ 78,F 51 0231: DECFSZ 78,F
52 025C: GOTO 258 52 0232: GOTO 22E
53 025D: MOVLW 97 53 0233: MOVLW 97
54 025E: MOVWF 77 54 0234: MOVWF 77
55 025F: DECFSZ 77,F 55 0235: DECFSZ 77,F
56 0260: GOTO 25F 56 0236: GOTO 235
57 0261: DECFSZ 00,F 57 0237: DECFSZ 00,F
58 0262: GOTO 256 58 0238: GOTO 22C
59 0263: BCF 0A.3 59 0239: BCF 0A.3
60 0264: BCF 0A.4 60 023A: BCF 0A.4
61 0265: GOTO 31D (RETURN) 61 023B: GOTO 2F8 (RETURN)
62 .................... #use i2c(Master,Slow,sda=PIN_C4,scl=PIN_C3) 62 .................... #use i2c(Master,Slow,sda=PIN_C4,scl=PIN_C3)
63 * 63 *
64 0089: MOVLW 08 64 0069: MOVLW 08
65 008A: MOVWF 78 65 006A: MOVWF 78
66 008B: NOP 66 006B: NOP
67 008C: BCF 07.3 67 006C: BCF 07.3
68 008D: BCF 20.3 68 006D: BCF 20.3
69 008E: MOVF 20,W 69 006E: MOVF 20,W
70 008F: BSF 03.5 70 006F: BSF 03.5
71 0090: MOVWF 07 71 0070: MOVWF 07
72 0091: NOP 72 0071: NOP
73 0092: BCF 03.5 73 0072: BCF 03.5
74 0093: RLF 3D,F 74 0073: RLF 2D,F
75 0094: BCF 07.4 75 0074: BCF 07.4
76 0095: BTFSS 03.0 76 0075: BTFSS 03.0
77 0096: GOTO 09D 77 0076: GOTO 07D
78 0097: BSF 20.4 78 0077: BSF 20.4
79 0098: MOVF 20,W 79 0078: MOVF 20,W
80 0099: BSF 03.5 80 0079: BSF 03.5
81 009A: MOVWF 07 81 007A: MOVWF 07
82 009B: GOTO 0A1 82 007B: GOTO 081
83 009C: BCF 03.5 83 007C: BCF 03.5
84 009D: BCF 20.4 84 007D: BCF 20.4
85 009E: MOVF 20,W 85 007E: MOVF 20,W
86 009F: BSF 03.5 86 007F: BSF 03.5
87 00A0: MOVWF 07 87 0080: MOVWF 07
88 00A1: NOP 88 0081: NOP
89 00A2: BCF 03.5 89 0082: BCF 03.5
90 00A3: BSF 20.3 90 0083: BSF 20.3
91 00A4: MOVF 20,W 91 0084: MOVF 20,W
92 00A5: BSF 03.5 92 0085: BSF 03.5
93 00A6: MOVWF 07 93 0086: MOVWF 07
94 00A7: BCF 03.5 94 0087: BCF 03.5
95 00A8: BTFSS 07.3 95 0088: BTFSS 07.3
96 00A9: GOTO 0A8 96 0089: GOTO 088
97 00AA: DECFSZ 78,F 97 008A: DECFSZ 78,F
98 00AB: GOTO 08B 98 008B: GOTO 06B
99 00AC: NOP 99 008C: NOP
100 00AD: BCF 07.3 100 008D: BCF 07.3
101 00AE: BCF 20.3 101 008E: BCF 20.3
102 00AF: MOVF 20,W 102 008F: MOVF 20,W
103 00B0: BSF 03.5 103 0090: BSF 03.5
104 00B1: MOVWF 07 104 0091: MOVWF 07
105 00B2: NOP 105 0092: NOP
106 00B3: BCF 03.5 106 0093: BCF 03.5
107 00B4: BSF 20.4 107 0094: BSF 20.4
108 00B5: MOVF 20,W 108 0095: MOVF 20,W
109 00B6: BSF 03.5 109 0096: BSF 03.5
110 00B7: MOVWF 07 110 0097: MOVWF 07
111 00B8: NOP 111 0098: NOP
112 00B9: NOP 112 0099: NOP
113 00BA: BCF 03.5 113 009A: BCF 03.5
114 00BB: BSF 20.3 114 009B: BSF 20.3
115 00BC: MOVF 20,W 115 009C: MOVF 20,W
116 00BD: BSF 03.5 116 009D: BSF 03.5
117 00BE: MOVWF 07 117 009E: MOVWF 07
118 00BF: BCF 03.5 118 009F: BCF 03.5
119 00C0: BTFSS 07.3 119 00A0: BTFSS 07.3
120 00C1: GOTO 0C0 120 00A1: GOTO 0A0
121 00C2: CLRF 78 121 00A2: CLRF 78
122 00C3: NOP 122 00A3: NOP
123 00C4: BTFSC 07.4 123 00A4: BTFSC 07.4
124 00C5: BSF 78.0 124 00A5: BSF 78.0
125 00C6: BCF 07.3 125 00A6: BCF 07.3
126 00C7: BCF 20.3 126 00A7: BCF 20.3
127 00C8: MOVF 20,W 127 00A8: MOVF 20,W
128 00C9: BSF 03.5 128 00A9: BSF 03.5
129 00CA: MOVWF 07 129 00AA: MOVWF 07
130 00CB: BCF 03.5 130 00AB: BCF 03.5
131 00CC: BCF 07.4 131 00AC: BCF 07.4
132 00CD: BCF 20.4 132 00AD: BCF 20.4
133 00CE: MOVF 20,W 133 00AE: MOVF 20,W
134 00CF: BSF 03.5 134 00AF: BSF 03.5
135 00D0: MOVWF 07 135 00B0: MOVWF 07
136 00D1: BCF 03.5 136 00B1: BCF 03.5
137 00D2: RETURN 137 00B2: RETURN
138 * 138 *
139 010C: MOVLW 08 139 00EC: MOVLW 08
140 010D: MOVWF 3E 140 00ED: MOVWF 2E
141 010E: MOVF 77,W 141 00EE: MOVF 77,W
142 010F: MOVWF 3F 142 00EF: MOVWF 2F
143 0110: BSF 20.4 143 00F0: BSF 20.4
144 0111: MOVF 20,W 144 00F1: MOVF 20,W
145 0112: BSF 03.5 145 00F2: BSF 03.5
146 0113: MOVWF 07 146 00F3: MOVWF 07
147 0114: NOP 147 00F4: NOP
148 0115: BCF 03.5 148 00F5: BCF 03.5
149 0116: BSF 20.3 149 00F6: BSF 20.3
150 0117: MOVF 20,W 150 00F7: MOVF 20,W
151 0118: BSF 03.5 151 00F8: BSF 03.5
152 0119: MOVWF 07 152 00F9: MOVWF 07
153 011A: BCF 03.5 153 00FA: BCF 03.5
154 011B: BTFSS 07.3 154 00FB: BTFSS 07.3
155 011C: GOTO 11B 155 00FC: GOTO 0FB
156 011D: BTFSC 07.4 156 00FD: BTFSC 07.4
157 011E: BSF 03.0 157 00FE: BSF 03.0
158 011F: BTFSS 07.4 158 00FF: BTFSS 07.4
159 0120: BCF 03.0 159 0100: BCF 03.0
160 0121: RLF 78,F 160 0101: RLF 78,F
-   161 0102: NOP
-   162 0103: BCF 20.3
-   163 0104: MOVF 20,W
-   164 0105: BSF 03.5
-   165 0106: MOVWF 07
-   166 0107: BCF 03.5
-   167 0108: BCF 07.3
-   168 0109: DECFSZ 2E,F
-   169 010A: GOTO 0F0
-   170 010B: BSF 20.4
-   171 010C: MOVF 20,W
-   172 010D: BSF 03.5
-   173 010E: MOVWF 07
-   174 010F: NOP
-   175 0110: BCF 03.5
-   176 0111: BCF 07.4
-   177 0112: MOVF 2F,W
-   178 0113: BTFSC 03.2
-   179 0114: GOTO 11A
-   180 0115: BCF 20.4
-   181 0116: MOVF 20,W
-   182 0117: BSF 03.5
-   183 0118: MOVWF 07
-   184 0119: BCF 03.5
-   185 011A: NOP
-   186 011B: BSF 20.3
-   187 011C: MOVF 20,W
-   188 011D: BSF 03.5
-   189 011E: MOVWF 07
-   190 011F: BCF 03.5
-   191 0120: BTFSS 07.3
-   192 0121: GOTO 120
161 0122: NOP 193 0122: NOP
-   194 0123: BCF 07.3
162 0123: BCF 20.3 195 0124: BCF 20.3
163 0124: MOVF 20,W 196 0125: MOVF 20,W
164 0125: BSF 03.5 197 0126: BSF 03.5
165 0126: MOVWF 07 198 0127: MOVWF 07
166 0127: BCF 03.5 199 0128: NOP
167 0128: BCF 07.3 200 0129: BCF 03.5
168 0129: DECFSZ 3E,F -  
169 012A: GOTO 110 201 012A: BCF 07.4
170 012B: BSF 20.4 202 012B: BCF 20.4
171 012C: MOVF 20,W 203 012C: MOVF 20,W
172 012D: BSF 03.5 204 012D: BSF 03.5
173 012E: MOVWF 07 205 012E: MOVWF 07
174 012F: NOP -  
175 0130: BCF 03.5 -  
176 0131: BCF 07.4 -  
177 0132: MOVF 3F,W -  
178 0133: BTFSC 03.2 -  
179 0134: GOTO 13A -  
180 0135: BCF 20.4 -  
181 0136: MOVF 20,W -  
182 0137: BSF 03.5 -  
183 0138: MOVWF 07 -  
184 0139: BCF 03.5 -  
185 013A: NOP -  
186 013B: BSF 20.3 -  
187 013C: MOVF 20,W -  
188 013D: BSF 03.5 -  
189 013E: MOVWF 07 -  
190 013F: BCF 03.5 206 012F: BCF 03.5
191 0140: BTFSS 07.3 -  
192 0141: GOTO 140 -  
193 0142: NOP -  
194 0143: BCF 07.3 -  
195 0144: BCF 20.3 -  
196 0145: MOVF 20,W -  
197 0146: BSF 03.5 -  
198 0147: MOVWF 07 -  
199 0148: NOP -  
200 0149: BCF 03.5 -  
201 014A: BCF 07.4 -  
202 014B: BCF 20.4 -  
203 014C: MOVF 20,W -  
204 014D: BSF 03.5 -  
205 014E: MOVWF 07 -  
206 014F: BCF 03.5 -  
207 0150: RETURN 207 0130: RETURN
208 .................... #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD 208 .................... #use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8) //rcv TXD xmit RXD
209 * -  
210 001F: BCF 20.7 -  
211 0020: MOVF 20,W -  
212 0021: BSF 03.5 -  
213 0022: MOVWF 07 -  
214 0023: BCF 03.5 -  
215 0024: BCF 07.7 -  
216 0025: MOVLW 08 -  
217 0026: MOVWF 78 -  
218 0027: GOTO 028 -  
219 0028: NOP -  
220 0029: BSF 78.7 -  
221 002A: GOTO 039 -  
222 002B: BCF 78.7 -  
223 002C: RRF 40,F -  
224 002D: BTFSC 03.0 -  
225 002E: BSF 07.7 -  
226 002F: BTFSS 03.0 -  
227 0030: BCF 07.7 -  
228 0031: BSF 78.6 -  
229 0032: GOTO 039 -  
230 0033: BCF 78.6 -  
231 0034: DECFSZ 78,F -  
232 0035: GOTO 02C -  
233 0036: GOTO 037 -  
234 0037: NOP -  
235 0038: BSF 07.7 -  
236 0039: MOVLW 3F -  
237 003A: MOVWF 04 -  
238 003B: DECFSZ 04,F -  
239 003C: GOTO 03B -  
240 003D: NOP -  
241 003E: BTFSC 78.7 -  
242 003F: GOTO 02B -  
243 0040: BTFSC 78.6 -  
244 0041: GOTO 033 -  
245 0042: RETURN -  
246 .................... 209 ....................
247 .................... 210 ....................
248 .................... #include "HMC5883L.h" 211 .................... #include "HMC5883L.h"
249 .................... // i2c slave addresses 212 .................... // i2c slave addresses
250 .................... #define HMC5883L_WRT_ADDR 0x3C 213 .................... #define HMC5883L_WRT_ADDR 0x3C
251 .................... #define HMC5883L_READ_ADDR 0x3D 214 .................... #define HMC5883L_READ_ADDR 0x3D
252 .................... 215 ....................
253 .................... // Register addresses 216 .................... // Register addresses
254 .................... #define HMC5883L_CFG_A_REG 0x00 217 .................... #define HMC5883L_CFG_A_REG 0x00
255 .................... #define HMC5883L_CFG_B_REG 0x01 218 .................... #define HMC5883L_CFG_B_REG 0x01
256 .................... #define HMC5883L_MODE_REG 0x02 219 .................... #define HMC5883L_MODE_REG 0x02
257 .................... #define HMC5883L_X_MSB_REG 0x03 220 .................... #define HMC5883L_X_MSB_REG 0x03
258 .................... 221 ....................
259 .................... //Konstanty nastavujici rozsah 222 .................... //Konstanty nastavujici rozsah
260 .................... //pro void set_mag_roz (unsigned int8 h) 223 .................... //pro void set_mag_roz (unsigned int8 h)
261 .................... #define MAG_ROZ088 0x00 224 .................... #define MAG_ROZ088 0x00
262 .................... #define MAG_ROZ130 0x20 225 .................... #define MAG_ROZ130 0x20
263 .................... #define MAG_ROZ190 0x40 226 .................... #define MAG_ROZ190 0x40
264 .................... #define MAG_ROZ250 0x60 227 .................... #define MAG_ROZ250 0x60
265 .................... #define MAG_ROZ400 0x80 228 .................... #define MAG_ROZ400 0x80
266 .................... #define MAG_ROZ470 0xA0 229 .................... #define MAG_ROZ470 0xA0
267 .................... #define MAG_ROZ560 0xC0 230 .................... #define MAG_ROZ560 0xC0
268 .................... #define MAG_ROZ810 0xE0 231 .................... #define MAG_ROZ810 0xE0
269 .................... 232 ....................
270 .................... 233 ....................
271 .................... #include "HMC5883L.c" 234 .................... #include "HMC5883L.c"
272 .................... //------------------------------ 235 .................... //------------------------------
273 .................... // Low level routines 236 .................... // Low level routines
274 .................... //------------------------------ 237 .................... //------------------------------
275 .................... void hmc5883l_write_reg(int8 reg, int8 data) 238 .................... void hmc5883l_write_reg(int8 reg, int8 data)
276 .................... { 239 .................... {
277 .................... i2c_start(); 240 .................... i2c_start();
278 * 241 *
279 00D3: BSF 20.4 242 00B3: BSF 20.4
280 00D4: MOVF 20,W 243 00B4: MOVF 20,W
281 00D5: BSF 03.5 244 00B5: BSF 03.5
282 00D6: MOVWF 07 245 00B6: MOVWF 07
283 00D7: NOP 246 00B7: NOP
284 00D8: BCF 03.5 247 00B8: BCF 03.5
285 00D9: BSF 20.3 248 00B9: BSF 20.3
286 00DA: MOVF 20,W 249 00BA: MOVF 20,W
287 00DB: BSF 03.5 250 00BB: BSF 03.5
288 00DC: MOVWF 07 251 00BC: MOVWF 07
289 00DD: NOP 252 00BD: NOP
290 00DE: BCF 03.5 253 00BE: BCF 03.5
291 00DF: BCF 07.4 254 00BF: BCF 07.4
292 00E0: BCF 20.4 255 00C0: BCF 20.4
293 00E1: MOVF 20,W 256 00C1: MOVF 20,W
294 00E2: BSF 03.5 257 00C2: BSF 03.5
295 00E3: MOVWF 07 258 00C3: MOVWF 07
296 00E4: NOP 259 00C4: NOP
297 00E5: BCF 03.5 260 00C5: BCF 03.5
298 00E6: BCF 07.3 261 00C6: BCF 07.3
299 00E7: BCF 20.3 262 00C7: BCF 20.3
300 00E8: MOVF 20,W 263 00C8: MOVF 20,W
301 00E9: BSF 03.5 264 00C9: BSF 03.5
302 00EA: MOVWF 07 265 00CA: MOVWF 07
303 .................... i2c_write(HMC5883L_WRT_ADDR); 266 .................... i2c_write(HMC5883L_WRT_ADDR);
304 00EB: MOVLW 3C 267 00CB: MOVLW 3C
305 00EC: BCF 03.5 268 00CC: BCF 03.5
306 00ED: MOVWF 3D 269 00CD: MOVWF 2D
307 00EE: CALL 089 270 00CE: CALL 069
308 .................... i2c_write(reg); 271 .................... i2c_write(reg);
309 00EF: MOVF 37,W 272 00CF: MOVF 27,W
310 00F0: MOVWF 3D 273 00D0: MOVWF 2D
311 00F1: CALL 089 274 00D1: CALL 069
312 .................... i2c_write(data); 275 .................... i2c_write(data);
313 00F2: MOVF 38,W 276 00D2: MOVF 28,W
314 00F3: MOVWF 3D 277 00D3: MOVWF 2D
315 00F4: CALL 089 278 00D4: CALL 069
316 .................... i2c_stop(); 279 .................... i2c_stop();
317 00F5: BCF 20.4 280 00D5: BCF 20.4
318 00F6: MOVF 20,W 281 00D6: MOVF 20,W
319 00F7: BSF 03.5 282 00D7: BSF 03.5
320 00F8: MOVWF 07 283 00D8: MOVWF 07
321 00F9: NOP 284 00D9: NOP
322 00FA: BCF 03.5 285 00DA: BCF 03.5
323 00FB: BSF 20.3 286 00DB: BSF 20.3
324 00FC: MOVF 20,W 287 00DC: MOVF 20,W
325 00FD: BSF 03.5 288 00DD: BSF 03.5
326 00FE: MOVWF 07 289 00DE: MOVWF 07
327 00FF: BCF 03.5 290 00DF: BCF 03.5
328 0100: BTFSS 07.3 291 00E0: BTFSS 07.3
329 0101: GOTO 100 292 00E1: GOTO 0E0
330 0102: NOP 293 00E2: NOP
331 0103: GOTO 104 294 00E3: GOTO 0E4
332 0104: NOP 295 00E4: NOP
333 0105: BSF 20.4 296 00E5: BSF 20.4
334 0106: MOVF 20,W 297 00E6: MOVF 20,W
335 0107: BSF 03.5 298 00E7: BSF 03.5
336 0108: MOVWF 07 299 00E8: MOVWF 07
337 0109: NOP 300 00E9: NOP
338 .................... } 301 .................... }
339 010A: BCF 03.5 302 00EA: BCF 03.5
340 010B: RETURN 303 00EB: RETURN
341 .................... 304 ....................
342 .................... //------------------------------ 305 .................... //------------------------------
343 .................... int8 hmc5883l_read_reg(int8 reg) 306 .................... int8 hmc5883l_read_reg(int8 reg)
344 .................... { 307 .................... {
345 .................... int8 retval; 308 .................... int8 retval;
346 .................... 309 ....................
347 .................... i2c_start(); 310 .................... i2c_start();
348 .................... i2c_write(HMC5883L_WRT_ADDR); 311 .................... i2c_write(HMC5883L_WRT_ADDR);
349 .................... i2c_write(reg); 312 .................... i2c_write(reg);
350 .................... i2c_start(); 313 .................... i2c_start();
351 .................... i2c_write(HMC5883L_READ_ADDR); 314 .................... i2c_write(HMC5883L_READ_ADDR);
352 .................... retval = i2c_read(0); 315 .................... retval = i2c_read(0);
353 .................... i2c_stop(); 316 .................... i2c_stop();
354 .................... 317 ....................
355 .................... return(retval); 318 .................... return(retval);
356 .................... } 319 .................... }
357 .................... 320 ....................
358 .................... //------------------------------ 321 .................... //------------------------------
359 .................... typedef struct 322 .................... typedef struct
360 .................... { 323 .................... {
361 .................... signed int16 x; 324 .................... signed int16 x;
362 .................... signed int16 y; 325 .................... signed int16 y;
363 .................... signed int16 z; 326 .................... signed int16 z;
364 .................... }hmc5883l_result; 327 .................... }hmc5883l_result;
365 .................... 328 ....................
366 .................... // This global structure holds the values read 329 .................... // This global structure holds the values read
367 .................... // from the HMC5883L x,y,z registers. 330 .................... // from the HMC5883L x,y,z registers.
368 .................... hmc5883l_result compass = {0,0,0}; 331 .................... hmc5883l_result compass = {0,0,0};
369 * 332 *
370 0287: CLRF 21 333 0260: CLRF 21
371 0288: CLRF 22 334 0261: CLRF 22
372 0289: CLRF 23 335 0262: CLRF 23
373 028A: CLRF 24 336 0263: CLRF 24
374 028B: CLRF 25 337 0264: CLRF 25
375 028C: CLRF 26 338 0265: CLRF 26
376 .................... 339 ....................
377 .................... //------------------------------ 340 .................... //------------------------------
378 .................... void hmc5883l_read_data(void) 341 .................... void hmc5883l_read_data(void)
379 .................... { 342 .................... {
380 .................... unsigned int8 x_lsb; 343 .................... unsigned int8 x_lsb;
381 .................... unsigned int8 x_msb; 344 .................... unsigned int8 x_msb;
382 .................... 345 ....................
383 .................... unsigned int8 y_lsb; 346 .................... unsigned int8 y_lsb;
384 .................... unsigned int8 y_msb; 347 .................... unsigned int8 y_msb;
385 .................... 348 ....................
386 .................... unsigned int8 z_lsb; 349 .................... unsigned int8 z_lsb;
387 .................... unsigned int8 z_msb; 350 .................... unsigned int8 z_msb;
388 .................... 351 ....................
389 .................... i2c_start(); 352 .................... i2c_start();
390 * 353 *
391 0151: BSF 20.4 354 0131: BSF 20.4
392 0152: MOVF 20,W 355 0132: MOVF 20,W
393 0153: BSF 03.5 356 0133: BSF 03.5
394 0154: MOVWF 07 357 0134: MOVWF 07
395 0155: NOP 358 0135: NOP
396 0156: BCF 03.5 359 0136: BCF 03.5
397 0157: BSF 20.3 360 0137: BSF 20.3
398 0158: MOVF 20,W 361 0138: MOVF 20,W
399 0159: BSF 03.5 362 0139: BSF 03.5
400 015A: MOVWF 07 363 013A: MOVWF 07
401 015B: NOP 364 013B: NOP
402 015C: BCF 03.5 365 013C: BCF 03.5
403 015D: BCF 07.4 366 013D: BCF 07.4
404 015E: BCF 20.4 367 013E: BCF 20.4
405 015F: MOVF 20,W 368 013F: MOVF 20,W
406 0160: BSF 03.5 369 0140: BSF 03.5
407 0161: MOVWF 07 370 0141: MOVWF 07
408 0162: NOP 371 0142: NOP
409 0163: BCF 03.5 372 0143: BCF 03.5
410 0164: BCF 07.3 373 0144: BCF 07.3
411 0165: BCF 20.3 374 0145: BCF 20.3
412 0166: MOVF 20,W 375 0146: MOVF 20,W
413 0167: BSF 03.5 376 0147: BSF 03.5
414 0168: MOVWF 07 377 0148: MOVWF 07
415 .................... i2c_write(HMC5883L_WRT_ADDR); 378 .................... i2c_write(HMC5883L_WRT_ADDR);
416 0169: MOVLW 3C 379 0149: MOVLW 3C
417 016A: BCF 03.5 380 014A: BCF 03.5
418 016B: MOVWF 3D 381 014B: MOVWF 2D
419 016C: CALL 089 382 014C: CALL 069
420 .................... i2c_write(HMC5883L_X_MSB_REG); // Point to X-msb register 383 .................... i2c_write(HMC5883L_X_MSB_REG); // Point to X-msb register
421 016D: MOVLW 03 384 014D: MOVLW 03
422 016E: MOVWF 3D 385 014E: MOVWF 2D
423 016F: CALL 089 386 014F: CALL 069
424 .................... i2c_start(); 387 .................... i2c_start();
425 0170: BSF 20.4 388 0150: BSF 20.4
426 0171: MOVF 20,W 389 0151: MOVF 20,W
427 0172: BSF 03.5 390 0152: BSF 03.5
428 0173: MOVWF 07 391 0153: MOVWF 07
429 0174: NOP 392 0154: NOP
430 0175: BCF 03.5 393 0155: BCF 03.5
431 0176: BSF 20.3 394 0156: BSF 20.3
432 0177: MOVF 20,W 395 0157: MOVF 20,W
433 0178: BSF 03.5 396 0158: BSF 03.5
434 0179: MOVWF 07 397 0159: MOVWF 07
435 017A: NOP 398 015A: NOP
436 017B: BCF 03.5 399 015B: BCF 03.5
437 017C: BTFSS 07.3 400 015C: BTFSS 07.3
438 017D: GOTO 17C 401 015D: GOTO 15C
439 017E: BCF 07.4 402 015E: BCF 07.4
440 017F: BCF 20.4 403 015F: BCF 20.4
441 0180: MOVF 20,W 404 0160: MOVF 20,W
442 0181: BSF 03.5 405 0161: BSF 03.5
443 0182: MOVWF 07 406 0162: MOVWF 07
444 0183: NOP 407 0163: NOP
445 0184: BCF 03.5 408 0164: BCF 03.5
446 0185: BCF 07.3 409 0165: BCF 07.3
447 0186: BCF 20.3 410 0166: BCF 20.3
448 0187: MOVF 20,W 411 0167: MOVF 20,W
449 0188: BSF 03.5 412 0168: BSF 03.5
450 0189: MOVWF 07 413 0169: MOVWF 07
451 .................... i2c_write(HMC5883L_READ_ADDR); 414 .................... i2c_write(HMC5883L_READ_ADDR);
452 018A: MOVLW 3D 415 016A: MOVLW 3D
453 018B: BCF 03.5 416 016B: BCF 03.5
454 018C: MOVWF 3D 417 016C: MOVWF 2D
455 018D: CALL 089 418 016D: CALL 069
456 .................... 419 ....................
457 .................... x_msb = i2c_read(); 420 .................... x_msb = i2c_read();
458 018E: MOVLW 01 421 016E: MOVLW 01
459 018F: MOVWF 77 422 016F: MOVWF 77
460 0190: CALL 10C 423 0170: CALL 0EC
461 0191: MOVF 78,W 424 0171: MOVF 78,W
462 0192: MOVWF 38 425 0172: MOVWF 28
463 .................... x_lsb = i2c_read(); 426 .................... x_lsb = i2c_read();
464 0193: MOVLW 01 427 0173: MOVLW 01
465 0194: MOVWF 77 428 0174: MOVWF 77
466 0195: CALL 10C 429 0175: CALL 0EC
467 0196: MOVF 78,W 430 0176: MOVF 78,W
468 0197: MOVWF 37 431 0177: MOVWF 27
469 .................... 432 ....................
470 .................... z_msb = i2c_read(); 433 .................... z_msb = i2c_read();
471 0198: MOVLW 01 434 0178: MOVLW 01
472 0199: MOVWF 77 435 0179: MOVWF 77
473 019A: CALL 10C 436 017A: CALL 0EC
474 019B: MOVF 78,W 437 017B: MOVF 78,W
475 019C: MOVWF 3C 438 017C: MOVWF 2C
476 .................... z_lsb = i2c_read(); 439 .................... z_lsb = i2c_read();
477 019D: MOVLW 01 440 017D: MOVLW 01
478 019E: MOVWF 77 441 017E: MOVWF 77
479 019F: CALL 10C 442 017F: CALL 0EC
480 01A0: MOVF 78,W 443 0180: MOVF 78,W
481 01A1: MOVWF 3B 444 0181: MOVWF 2B
482 .................... 445 ....................
483 .................... y_msb = i2c_read(); 446 .................... y_msb = i2c_read();
484 01A2: MOVLW 01 447 0182: MOVLW 01
485 01A3: MOVWF 77 448 0183: MOVWF 77
486 01A4: CALL 10C 449 0184: CALL 0EC
487 01A5: MOVF 78,W 450 0185: MOVF 78,W
488 01A6: MOVWF 3A 451 0186: MOVWF 2A
489 .................... y_lsb = i2c_read(0); // do a NACK on last read 452 .................... y_lsb = i2c_read(0); // do a NACK on last read
490 01A7: CLRF 77 453 0187: CLRF 77
491 01A8: CALL 10C 454 0188: CALL 0EC
492 01A9: MOVF 78,W 455 0189: MOVF 78,W
493 01AA: MOVWF 39 456 018A: MOVWF 29
494 .................... 457 ....................
495 .................... i2c_stop(); 458 .................... i2c_stop();
496 01AB: BCF 20.4 459 018B: BCF 20.4
497 01AC: MOVF 20,W 460 018C: MOVF 20,W
498 01AD: BSF 03.5 461 018D: BSF 03.5
499 01AE: MOVWF 07 462 018E: MOVWF 07
500 01AF: NOP 463 018F: NOP
501 01B0: BCF 03.5 464 0190: BCF 03.5
502 01B1: BSF 20.3 465 0191: BSF 20.3
503 01B2: MOVF 20,W 466 0192: MOVF 20,W
504 01B3: BSF 03.5 467 0193: BSF 03.5
505 01B4: MOVWF 07 468 0194: MOVWF 07
506 01B5: BCF 03.5 469 0195: BCF 03.5
507 01B6: BTFSS 07.3 470 0196: BTFSS 07.3
508 01B7: GOTO 1B6 471 0197: GOTO 196
509 01B8: NOP 472 0198: NOP
510 01B9: GOTO 1BA 473 0199: GOTO 19A
511 01BA: NOP 474 019A: NOP
512 01BB: BSF 20.4 475 019B: BSF 20.4
513 01BC: MOVF 20,W 476 019C: MOVF 20,W
514 01BD: BSF 03.5 477 019D: BSF 03.5
515 01BE: MOVWF 07 478 019E: MOVWF 07
516 01BF: NOP 479 019F: NOP
517 .................... 480 ....................
518 .................... // Combine high and low bytes into 16-bit values. 481 .................... // Combine high and low bytes into 16-bit values.
519 .................... compass.x = make16(x_msb, x_lsb); 482 .................... compass.x = make16(x_msb, x_lsb);
520 01C0: BCF 03.5 483 01A0: BCF 03.5
521 01C1: MOVF 38,W 484 01A1: MOVF 28,W
522 01C2: MOVWF 22 485 01A2: MOVWF 22
523 01C3: MOVF 37,W 486 01A3: MOVF 27,W
524 01C4: MOVWF 21 487 01A4: MOVWF 21
525 .................... compass.y = make16(y_msb, y_lsb); 488 .................... compass.y = make16(y_msb, y_lsb);
526 01C5: MOVF 3A,W 489 01A5: MOVF 2A,W
527 01C6: MOVWF 24 490 01A6: MOVWF 24
528 01C7: MOVF 39,W 491 01A7: MOVF 29,W
529 01C8: MOVWF 23 492 01A8: MOVWF 23
530 .................... compass.z = make16(z_msb, z_lsb); 493 .................... compass.z = make16(z_msb, z_lsb);
531 01C9: MOVF 3C,W 494 01A9: MOVF 2C,W
532 01CA: MOVWF 26 495 01AA: MOVWF 26
533 01CB: MOVF 3B,W 496 01AB: MOVF 2B,W
534 01CC: MOVWF 25 497 01AC: MOVWF 25
535 .................... } 498 .................... }
536 01CD: BCF 0A.3 499 01AD: BCF 0A.3
537 01CE: BCF 0A.4 500 01AE: BCF 0A.4
538 01CF: GOTO 2F6 (RETURN) 501 01AF: GOTO 2CC (RETURN)
539 .................... 502 ....................
540 .................... 503 ....................
541 .................... 504 ....................
542 .................... 505 ....................
543 .................... #include <math.h> 506 .................... #include <math.h>
544 .................... //////////////////////////////////////////////////////////////////////////// 507 .................... ////////////////////////////////////////////////////////////////////////////
545 .................... //// (C) Copyright 1996,2008 Custom Computer Services //// 508 .................... //// (C) Copyright 1996,2008 Custom Computer Services ////
546 .................... //// This source code may only be used by licensed users of the CCS C //// 509 .................... //// This source code may only be used by licensed users of the CCS C ////
547 .................... //// compiler. This source code may only be distributed to other //// 510 .................... //// compiler. This source code may only be distributed to other ////
548 .................... //// licensed users of the CCS C compiler. No other use, reproduction //// 511 .................... //// licensed users of the CCS C compiler. No other use, reproduction ////
549 .................... //// or distribution is permitted without written permission. //// 512 .................... //// or distribution is permitted without written permission. ////
550 .................... //// Derivative programs created using this software in object code //// 513 .................... //// Derivative programs created using this software in object code ////
551 .................... //// form are not restricted in any way. //// 514 .................... //// form are not restricted in any way. ////
552 .................... //////////////////////////////////////////////////////////////////////////// 515 .................... ////////////////////////////////////////////////////////////////////////////
553 .................... //// //// 516 .................... //// ////
554 .................... //// History: //// 517 .................... //// History: ////
555 .................... //// * 9/20/2001 : Improvments are made to sin/cos code. //// 518 .................... //// * 9/20/2001 : Improvments are made to sin/cos code. ////
556 .................... //// The code now is small, much faster, //// 519 .................... //// The code now is small, much faster, ////
557 .................... //// and more accurate. //// 520 .................... //// and more accurate. ////
558 .................... //// * 2/21/2007 : Compiler handles & operator differently and does 521 .................... //// * 2/21/2007 : Compiler handles & operator differently and does
559 .................... //// not return generic (int8 *) so type cast is done //// 522 .................... //// not return generic (int8 *) so type cast is done ////
560 .................... //// //// 523 .................... //// ////
561 .................... //////////////////////////////////////////////////////////////////////////// 524 .................... ////////////////////////////////////////////////////////////////////////////
562 .................... 525 ....................
563 .................... #ifndef MATH_H 526 .................... #ifndef MATH_H
564 .................... #define MATH_H 527 .................... #define MATH_H
565 .................... 528 ....................
566 .................... #ifdef PI 529 .................... #ifdef PI
567 .................... #undef PI 530 .................... #undef PI
568 .................... #endif 531 .................... #endif
569 .................... #define PI 3.1415926535897932 532 .................... #define PI 3.1415926535897932
570 .................... 533 ....................
571 .................... 534 ....................
572 .................... #define SQRT2 1.4142135623730950 535 .................... #define SQRT2 1.4142135623730950
573 .................... 536 ....................
574 .................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279}; 537 .................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279};
575 .................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718}; 538 .................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718};
576 .................... 539 ....................
577 .................... ///////////////////////////// Round Functions ////////////////////////////// 540 .................... ///////////////////////////// Round Functions //////////////////////////////
578 .................... 541 ....................
579 .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n) 542 .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n)
580 .................... { 543 .................... {
581 .................... float32 y, res; 544 .................... float32 y, res;
582 .................... unsigned int16 l; 545 .................... unsigned int16 l;
583 .................... int1 s; 546 .................... int1 s;
584 .................... 547 ....................
585 .................... s = 0; 548 .................... s = 0;
586 .................... y = x; 549 .................... y = x;
587 .................... 550 ....................
588 .................... if (x < 0) 551 .................... if (x < 0)
589 .................... { 552 .................... {
590 .................... s = 1; 553 .................... s = 1;
591 .................... y = -y; 554 .................... y = -y;
592 .................... } 555 .................... }
593 .................... 556 ....................
594 .................... if (y <= 32768.0) 557 .................... if (y <= 32768.0)
595 .................... res = (float32)(unsigned int16)y; 558 .................... res = (float32)(unsigned int16)y;
596 .................... 559 ....................
597 .................... else if (y < 10000000.0) 560 .................... else if (y < 10000000.0)
598 .................... { 561 .................... {
599 .................... l = (unsigned int16)(y/32768.0); 562 .................... l = (unsigned int16)(y/32768.0);
600 .................... y = 32768.0*(y/32768.0 - (float32)l); 563 .................... y = 32768.0*(y/32768.0 - (float32)l);
601 .................... res = 32768.0*(float32)l; 564 .................... res = 32768.0*(float32)l;
602 .................... res += (float32)(unsigned int16)y; 565 .................... res += (float32)(unsigned int16)y;
603 .................... } 566 .................... }
604 .................... 567 ....................
605 .................... else 568 .................... else
606 .................... res = y; 569 .................... res = y;
607 .................... 570 ....................
608 .................... y = y - (float32)(unsigned int16)y; 571 .................... y = y - (float32)(unsigned int16)y;
609 .................... 572 ....................
610 .................... if (s) 573 .................... if (s)
611 .................... res = -res; 574 .................... res = -res;
612 .................... 575 ....................
613 .................... if (y != 0) 576 .................... if (y != 0)
614 .................... { 577 .................... {
615 .................... if (s == 1 && n == 0) 578 .................... if (s == 1 && n == 0)
616 .................... res -= 1.0; 579 .................... res -= 1.0;
617 .................... 580 ....................
618 .................... if (s == 0 && n == 1) 581 .................... if (s == 0 && n == 1)
619 .................... res += 1.0; 582 .................... res += 1.0;
620 .................... } 583 .................... }
621 .................... if (x == 0) 584 .................... if (x == 0)
622 .................... res = 0; 585 .................... res = 0;
623 .................... 586 ....................
624 .................... return (res); 587 .................... return (res);
625 .................... } 588 .................... }
626 .................... 589 ....................
627 .................... // Overloaded Functions to take care for new Data types in PCD 590 .................... // Overloaded Functions to take care for new Data types in PCD
628 .................... // Overloaded function CEIL_FLOOR() for data type - Float48 591 .................... // Overloaded function CEIL_FLOOR() for data type - Float48
629 .................... #if defined(__PCD__) 592 .................... #if defined(__PCD__)
630 .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n) 593 .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n)
631 .................... { 594 .................... {
632 .................... float48 y, res; 595 .................... float48 y, res;
633 .................... unsigned int16 l; 596 .................... unsigned int16 l;
634 .................... int1 s; 597 .................... int1 s;
635 .................... 598 ....................
636 .................... s = 0; 599 .................... s = 0;
637 .................... y = x; 600 .................... y = x;
638 .................... 601 ....................
639 .................... if (x < 0) 602 .................... if (x < 0)
640 .................... { 603 .................... {
641 .................... s = 1; 604 .................... s = 1;
642 .................... y = -y; 605 .................... y = -y;
643 .................... } 606 .................... }
644 .................... 607 ....................
645 .................... if (y <= 32768.0) 608 .................... if (y <= 32768.0)
646 .................... res = (float48)(unsigned int16)y; 609 .................... res = (float48)(unsigned int16)y;
647 .................... 610 ....................
648 .................... else if (y < 10000000.0) 611 .................... else if (y < 10000000.0)
649 .................... { 612 .................... {
650 .................... l = (unsigned int16)(y/32768.0); 613 .................... l = (unsigned int16)(y/32768.0);
651 .................... y = 32768.0*(y/32768.0 - (float48)l); 614 .................... y = 32768.0*(y/32768.0 - (float48)l);
652 .................... res = 32768.0*(float32)l; 615 .................... res = 32768.0*(float32)l;
653 .................... res += (float48)(unsigned int16)y; 616 .................... res += (float48)(unsigned int16)y;
654 .................... } 617 .................... }
655 .................... 618 ....................
656 .................... else 619 .................... else
657 .................... res = y; 620 .................... res = y;
658 .................... 621 ....................
659 .................... y = y - (float48)(unsigned int16)y; 622 .................... y = y - (float48)(unsigned int16)y;
660 .................... 623 ....................
661 .................... if (s) 624 .................... if (s)
662 .................... res = -res; 625 .................... res = -res;
663 .................... 626 ....................
664 .................... if (y != 0) 627 .................... if (y != 0)
665 .................... { 628 .................... {
666 .................... if (s == 1 && n == 0) 629 .................... if (s == 1 && n == 0)
667 .................... res -= 1.0; 630 .................... res -= 1.0;
668 .................... 631 ....................
669 .................... if (s == 0 && n == 1) 632 .................... if (s == 0 && n == 1)
670 .................... res += 1.0; 633 .................... res += 1.0;
671 .................... } 634 .................... }
672 .................... if (x == 0) 635 .................... if (x == 0)
673 .................... res = 0; 636 .................... res = 0;
674 .................... 637 ....................
675 .................... return (res); 638 .................... return (res);
676 .................... } 639 .................... }
677 .................... 640 ....................
678 .................... 641 ....................
679 .................... // Overloaded function CEIL_FLOOR() for data type - Float64 642 .................... // Overloaded function CEIL_FLOOR() for data type - Float64
680 .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n) 643 .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n)
681 .................... { 644 .................... {
682 .................... float64 y, res; 645 .................... float64 y, res;
683 .................... unsigned int16 l; 646 .................... unsigned int16 l;
684 .................... int1 s; 647 .................... int1 s;
685 .................... 648 ....................
686 .................... s = 0; 649 .................... s = 0;
687 .................... y = x; 650 .................... y = x;
688 .................... 651 ....................
689 .................... if (x < 0) 652 .................... if (x < 0)
690 .................... { 653 .................... {
691 .................... s = 1; 654 .................... s = 1;
692 .................... y = -y; 655 .................... y = -y;
693 .................... } 656 .................... }
694 .................... 657 ....................
695 .................... if (y <= 32768.0) 658 .................... if (y <= 32768.0)
696 .................... res = (float64)(unsigned int16)y; 659 .................... res = (float64)(unsigned int16)y;
697 .................... 660 ....................
698 .................... else if (y < 10000000.0) 661 .................... else if (y < 10000000.0)
699 .................... { 662 .................... {
700 .................... l = (unsigned int16)(y/32768.0); 663 .................... l = (unsigned int16)(y/32768.0);
701 .................... y = 32768.0*(y/32768.0 - (float64)l); 664 .................... y = 32768.0*(y/32768.0 - (float64)l);
702 .................... res = 32768.0*(float64)l; 665 .................... res = 32768.0*(float64)l;
703 .................... res += (float64)(unsigned int16)y; 666 .................... res += (float64)(unsigned int16)y;
704 .................... } 667 .................... }
705 .................... 668 ....................
706 .................... else 669 .................... else
707 .................... res = y; 670 .................... res = y;
708 .................... 671 ....................
709 .................... y = y - (float64)(unsigned int16)y; 672 .................... y = y - (float64)(unsigned int16)y;
710 .................... 673 ....................
711 .................... if (s) 674 .................... if (s)
712 .................... res = -res; 675 .................... res = -res;
713 .................... 676 ....................
714 .................... if (y != 0) 677 .................... if (y != 0)
715 .................... { 678 .................... {
716 .................... if (s == 1 && n == 0) 679 .................... if (s == 1 && n == 0)
717 .................... res -= 1.0; 680 .................... res -= 1.0;
718 .................... 681 ....................
719 .................... if (s == 0 && n == 1) 682 .................... if (s == 0 && n == 1)
720 .................... res += 1.0; 683 .................... res += 1.0;
721 .................... } 684 .................... }
722 .................... if (x == 0) 685 .................... if (x == 0)
723 .................... res = 0; 686 .................... res = 0;
724 .................... 687 ....................
725 .................... return (res); 688 .................... return (res);
726 .................... } 689 .................... }
727 .................... #endif 690 .................... #endif
728 .................... 691 ....................
729 .................... //////////////////////////////////////////////////////////////////////////// 692 .................... ////////////////////////////////////////////////////////////////////////////
730 .................... // float floor(float x) 693 .................... // float floor(float x)
731 .................... //////////////////////////////////////////////////////////////////////////// 694 .................... ////////////////////////////////////////////////////////////////////////////
732 .................... // Description : rounds down the number x. 695 .................... // Description : rounds down the number x.
733 .................... // Date : N/A 696 .................... // Date : N/A
734 .................... // 697 .................... //
735 .................... float32 floor(float32 x) 698 .................... float32 floor(float32 x)
736 .................... { 699 .................... {
737 .................... return CEIL_FLOOR(x, 0); 700 .................... return CEIL_FLOOR(x, 0);
738 .................... } 701 .................... }
739 .................... // Following 2 functions are overloaded functions of floor() for PCD 702 .................... // Following 2 functions are overloaded functions of floor() for PCD
740 .................... // Overloaded function floor() for data type - Float48 703 .................... // Overloaded function floor() for data type - Float48
741 .................... #if defined(__PCD__) 704 .................... #if defined(__PCD__)
742 .................... float48 floor(float48 x) 705 .................... float48 floor(float48 x)
743 .................... { 706 .................... {
744 .................... return CEIL_FLOOR(x, 0); 707 .................... return CEIL_FLOOR(x, 0);
745 .................... } 708 .................... }
746 .................... 709 ....................
747 .................... // Overloaded function floor() for data type - Float64 710 .................... // Overloaded function floor() for data type - Float64
748 .................... float64 floor(float64 x) 711 .................... float64 floor(float64 x)
749 .................... { 712 .................... {
750 .................... return CEIL_FLOOR(x, 0); 713 .................... return CEIL_FLOOR(x, 0);
751 .................... } 714 .................... }
752 .................... #endif 715 .................... #endif
753 .................... 716 ....................
754 .................... 717 ....................
755 .................... //////////////////////////////////////////////////////////////////////////// 718 .................... ////////////////////////////////////////////////////////////////////////////
756 .................... // float ceil(float x) 719 .................... // float ceil(float x)
757 .................... //////////////////////////////////////////////////////////////////////////// 720 .................... ////////////////////////////////////////////////////////////////////////////
758 .................... // Description : rounds up the number x. 721 .................... // Description : rounds up the number x.
759 .................... // Date : N/A 722 .................... // Date : N/A
760 .................... // 723 .................... //
761 .................... float32 ceil(float32 x) 724 .................... float32 ceil(float32 x)
762 .................... { 725 .................... {
763 .................... return CEIL_FLOOR(x, 1); 726 .................... return CEIL_FLOOR(x, 1);
764 .................... } 727 .................... }
765 .................... // Following 2 functions are overloaded functions of ceil() for PCD 728 .................... // Following 2 functions are overloaded functions of ceil() for PCD
766 .................... // Overloaded function ceil() for data type - Float48 729 .................... // Overloaded function ceil() for data type - Float48
767 .................... #if defined(__PCD__) 730 .................... #if defined(__PCD__)
768 .................... float48 ceil(float48 x) 731 .................... float48 ceil(float48 x)
769 .................... { 732 .................... {
770 .................... return CEIL_FLOOR(x, 1); 733 .................... return CEIL_FLOOR(x, 1);
771 .................... } 734 .................... }
772 .................... 735 ....................
773 .................... // Overloaded function ceil() for data type - Float64 736 .................... // Overloaded function ceil() for data type - Float64
774 .................... float64 ceil(float64 x) 737 .................... float64 ceil(float64 x)
775 .................... { 738 .................... {
776 .................... return CEIL_FLOOR(x, 1); 739 .................... return CEIL_FLOOR(x, 1);
777 .................... } 740 .................... }
778 .................... #endif 741 .................... #endif
779 .................... 742 ....................
780 .................... //////////////////////////////////////////////////////////////////////////// 743 .................... ////////////////////////////////////////////////////////////////////////////
781 .................... // float fabs(float x) 744 .................... // float fabs(float x)
782 .................... //////////////////////////////////////////////////////////////////////////// 745 .................... ////////////////////////////////////////////////////////////////////////////
783 .................... // Description : Computes the absolute value of floating point number x 746 .................... // Description : Computes the absolute value of floating point number x
784 .................... // Returns : returns the absolute value of x 747 .................... // Returns : returns the absolute value of x
785 .................... // Date : N/A 748 .................... // Date : N/A
786 .................... // 749 .................... //
787 .................... #define fabs abs 750 .................... #define fabs abs
788 .................... 751 ....................
789 .................... //////////////////////////////////////////////////////////////////////////// 752 .................... ////////////////////////////////////////////////////////////////////////////
790 .................... // float fmod(float x) 753 .................... // float fmod(float x)
791 .................... //////////////////////////////////////////////////////////////////////////// 754 .................... ////////////////////////////////////////////////////////////////////////////
792 .................... // Description : Computes the floating point remainder of x/y 755 .................... // Description : Computes the floating point remainder of x/y
793 .................... // Returns : returns the value of x= i*y, for some integer i such that, if y 756 .................... // Returns : returns the value of x= i*y, for some integer i such that, if y
794 .................... // is non zero, the result has the same isgn of x na dmagnitude less than the 757 .................... // is non zero, the result has the same isgn of x na dmagnitude less than the
795 .................... // magnitude of y. If y is zero then a domain error occurs. 758 .................... // magnitude of y. If y is zero then a domain error occurs.
796 .................... // Date : N/A 759 .................... // Date : N/A
797 .................... // 760 .................... //
798 .................... 761 ....................
799 .................... float fmod(float32 x,float32 y) 762 .................... float fmod(float32 x,float32 y)
800 .................... { 763 .................... {
801 .................... float32 i; 764 .................... float32 i;
802 .................... if (y!=0.0) 765 .................... if (y!=0.0)
803 .................... { 766 .................... {
804 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 767 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
805 .................... return(x-(i*y)); 768 .................... return(x-(i*y));
806 .................... } 769 .................... }
807 .................... else 770 .................... else
808 .................... { 771 .................... {
809 .................... #ifdef _ERRNO 772 .................... #ifdef _ERRNO
810 .................... { 773 .................... {
811 .................... errno=EDOM; 774 .................... errno=EDOM;
812 .................... } 775 .................... }
813 .................... #endif 776 .................... #endif
814 .................... } 777 .................... }
815 .................... } 778 .................... }
816 .................... //Overloaded function for fmod() for PCD 779 .................... //Overloaded function for fmod() for PCD
817 .................... // Overloaded function fmod() for data type - Float48 780 .................... // Overloaded function fmod() for data type - Float48
818 .................... #if defined(__PCD__) 781 .................... #if defined(__PCD__)
819 .................... float48 fmod(float48 x,float48 y) 782 .................... float48 fmod(float48 x,float48 y)
820 .................... { 783 .................... {
821 .................... float48 i; 784 .................... float48 i;
822 .................... if (y!=0.0) 785 .................... if (y!=0.0)
823 .................... { 786 .................... {
824 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 787 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
825 .................... return(x-(i*y)); 788 .................... return(x-(i*y));
826 .................... } 789 .................... }
827 .................... else 790 .................... else
828 .................... { 791 .................... {
829 .................... #ifdef _ERRNO 792 .................... #ifdef _ERRNO
830 .................... { 793 .................... {
831 .................... errno=EDOM; 794 .................... errno=EDOM;
832 .................... } 795 .................... }
833 .................... #endif 796 .................... #endif
834 .................... } 797 .................... }
835 .................... } 798 .................... }
836 .................... // Overloaded function fmod() for data type - Float64 799 .................... // Overloaded function fmod() for data type - Float64
837 .................... float64 fmod(float64 x,float64 y) 800 .................... float64 fmod(float64 x,float64 y)
838 .................... { 801 .................... {
839 .................... float64 i; 802 .................... float64 i;
840 .................... if (y!=0.0) 803 .................... if (y!=0.0)
841 .................... { 804 .................... {
842 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 805 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
843 .................... return(x-(i*y)); 806 .................... return(x-(i*y));
844 .................... } 807 .................... }
845 .................... else 808 .................... else
846 .................... { 809 .................... {
847 .................... #ifdef _ERRNO 810 .................... #ifdef _ERRNO
848 .................... { 811 .................... {
849 .................... errno=EDOM; 812 .................... errno=EDOM;
850 .................... } 813 .................... }
851 .................... #endif 814 .................... #endif
852 .................... } 815 .................... }
853 .................... } 816 .................... }
854 .................... #endif 817 .................... #endif
855 .................... //////////////////// Exponential and logarithmic functions //////////////////// 818 .................... //////////////////// Exponential and logarithmic functions ////////////////////
856 .................... //////////////////////////////////////////////////////////////////////////// 819 .................... ////////////////////////////////////////////////////////////////////////////
857 .................... // float exp(float x) 820 .................... // float exp(float x)
858 .................... //////////////////////////////////////////////////////////////////////////// 821 .................... ////////////////////////////////////////////////////////////////////////////
859 .................... // Description : returns the value (e^x) 822 .................... // Description : returns the value (e^x)
860 .................... // Date : N/A 823 .................... // Date : N/A
861 .................... // 824 .................... //
862 .................... #define LN2 0.6931471805599453 825 .................... #define LN2 0.6931471805599453
863 .................... 826 ....................
864 .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, 827 .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093,
865 .................... 0.0554965651, 0.240227138, 0.693147172}; 828 .................... 0.0554965651, 0.240227138, 0.693147172};
866 .................... 829 ....................
867 .................... 830 ....................
868 .................... float32 exp(float32 x) 831 .................... float32 exp(float32 x)
869 .................... { 832 .................... {
870 .................... float32 y, res, r; 833 .................... float32 y, res, r;
871 .................... #if defined(__PCD__) 834 .................... #if defined(__PCD__)
872 .................... int8 data1; 835 .................... int8 data1;
873 .................... #endif 836 .................... #endif
874 .................... signed int8 n; 837 .................... signed int8 n;
875 .................... int1 s; 838 .................... int1 s;
876 .................... #ifdef _ERRNO 839 .................... #ifdef _ERRNO
877 .................... if(x > 88.722838) 840 .................... if(x > 88.722838)
878 .................... { 841 .................... {
879 .................... errno=ERANGE; 842 .................... errno=ERANGE;
880 .................... return(0); 843 .................... return(0);
881 .................... } 844 .................... }
882 .................... #endif 845 .................... #endif
883 .................... n = (signed int16)(x/LN2); 846 .................... n = (signed int16)(x/LN2);
884 .................... s = 0; 847 .................... s = 0;
885 .................... y = x; 848 .................... y = x;
886 .................... 849 ....................
887 .................... if (x < 0) 850 .................... if (x < 0)
888 .................... { 851 .................... {
889 .................... s = 1; 852 .................... s = 1;
890 .................... n = -n; 853 .................... n = -n;
891 .................... y = -y; 854 .................... y = -y;
892 .................... } 855 .................... }
893 .................... 856 ....................
894 .................... res = 0.0; 857 .................... res = 0.0;
895 .................... #if !defined(__PCD__) 858 .................... #if !defined(__PCD__)
896 .................... *((unsigned int8 *)(&res)) = n + 0x7F; 859 .................... *((unsigned int8 *)(&res)) = n + 0x7F;
897 .................... #endif 860 .................... #endif
898 .................... 861 ....................
899 .................... #if defined(__PCD__) // Takes care of IEEE format for PCD 862 .................... #if defined(__PCD__) // Takes care of IEEE format for PCD
900 .................... data1 = n+0x7F; 863 .................... data1 = n+0x7F;
901 .................... if(bit_test(data1,0)) 864 .................... if(bit_test(data1,0))
902 .................... bit_set(*(((unsigned int8 *)(&res)+2)),7); 865 .................... bit_set(*(((unsigned int8 *)(&res)+2)),7);
903 .................... rotate_right(&data1,1); 866 .................... rotate_right(&data1,1);
904 .................... bit_clear(data1,7); 867 .................... bit_clear(data1,7);
905 .................... *(((unsigned int8 *)(&res)+3)) = data1; 868 .................... *(((unsigned int8 *)(&res)+3)) = data1;
906 .................... #endif 869 .................... #endif
907 .................... 870 ....................
908 .................... y = y/LN2 - (float32)n; 871 .................... y = y/LN2 - (float32)n;
909 .................... 872 ....................
910 .................... r = pe[0]*y + pe[1]; 873 .................... r = pe[0]*y + pe[1];
911 .................... r = r*y + pe[2]; 874 .................... r = r*y + pe[2];
912 .................... r = r*y + pe[3]; 875 .................... r = r*y + pe[3];
913 .................... r = r*y + pe[4]; 876 .................... r = r*y + pe[4];
914 .................... r = r*y + pe[5]; 877 .................... r = r*y + pe[5];
915 .................... 878 ....................
916 .................... res = res*(1.0 + y*r); 879 .................... res = res*(1.0 + y*r);
917 .................... 880 ....................
918 .................... if (s) 881 .................... if (s)
919 .................... res = 1.0/res; 882 .................... res = 1.0/res;
920 .................... return(res); 883 .................... return(res);
921 .................... } 884 .................... }
922 .................... 885 ....................
923 .................... 886 ....................
924 .................... //Overloaded function for exp() for PCD 887 .................... //Overloaded function for exp() for PCD
925 .................... // Overloaded function exp() for data type - Float48 888 .................... // Overloaded function exp() for data type - Float48
926 .................... #if defined(__PCD__) 889 .................... #if defined(__PCD__)
927 .................... float48 exp(float48 x) 890 .................... float48 exp(float48 x)
928 .................... { 891 .................... {
929 .................... float48 y, res, r; 892 .................... float48 y, res, r;
930 .................... int8 data1; 893 .................... int8 data1;
931 .................... signed int8 n; 894 .................... signed int8 n;
932 .................... int1 s; 895 .................... int1 s;
933 .................... #ifdef _ERRNO 896 .................... #ifdef _ERRNO
934 .................... if(x > 88.722838) 897 .................... if(x > 88.722838)
935 .................... { 898 .................... {
936 .................... errno=ERANGE; 899 .................... errno=ERANGE;
937 .................... return(0); 900 .................... return(0);
938 .................... } 901 .................... }
939 .................... #endif 902 .................... #endif
940 .................... n = (signed int16)(x/LN2); 903 .................... n = (signed int16)(x/LN2);
941 .................... s = 0; 904 .................... s = 0;
942 .................... y = x; 905 .................... y = x;
943 .................... 906 ....................
944 .................... if (x < 0) 907 .................... if (x < 0)
945 .................... { 908 .................... {
946 .................... s = 1; 909 .................... s = 1;
947 .................... n = -n; 910 .................... n = -n;
948 .................... y = -y; 911 .................... y = -y;
949 .................... } 912 .................... }
950 .................... 913 ....................
951 .................... res = 0.0; 914 .................... res = 0.0;
952 .................... 915 ....................
953 .................... data1 = n+0x7F; 916 .................... data1 = n+0x7F;
954 .................... if(bit_test(data1,0)) 917 .................... if(bit_test(data1,0))
955 .................... bit_set(*(((unsigned int8 *)(&res)+4)),7); 918 .................... bit_set(*(((unsigned int8 *)(&res)+4)),7);
956 .................... rotate_right(&data1,1); 919 .................... rotate_right(&data1,1);
957 .................... bit_clear(data1,7); 920 .................... bit_clear(data1,7);
958 .................... *(((unsigned int8 *)(&res)+5)) = data1; 921 .................... *(((unsigned int8 *)(&res)+5)) = data1;
959 .................... 922 ....................
960 .................... y = y/LN2 - (float48)n; 923 .................... y = y/LN2 - (float48)n;
961 .................... 924 ....................
962 .................... r = pe[0]*y + pe[1]; 925 .................... r = pe[0]*y + pe[1];
963 .................... r = r*y + pe[2]; 926 .................... r = r*y + pe[2];
964 .................... r = r*y + pe[3]; 927 .................... r = r*y + pe[3];
965 .................... r = r*y + pe[4]; 928 .................... r = r*y + pe[4];
966 .................... r = r*y + pe[5]; 929 .................... r = r*y + pe[5];
967 .................... 930 ....................
968 .................... res = res*(1.0 + y*r); 931 .................... res = res*(1.0 + y*r);
969 .................... 932 ....................
970 .................... if (s) 933 .................... if (s)
971 .................... res = 1.0/res; 934 .................... res = 1.0/res;
972 .................... return(res); 935 .................... return(res);
973 .................... } 936 .................... }
974 .................... 937 ....................
975 .................... // Overloaded function exp() for data type - Float64 938 .................... // Overloaded function exp() for data type - Float64
976 .................... float64 exp(float64 x) 939 .................... float64 exp(float64 x)
977 .................... { 940 .................... {
978 .................... float64 y, res, r; 941 .................... float64 y, res, r;
979 .................... unsigned int16 data1, data2; 942 .................... unsigned int16 data1, data2;
980 .................... unsigned int16 *p; 943 .................... unsigned int16 *p;
981 .................... signed int16 n; 944 .................... signed int16 n;
982 .................... int1 s; 945 .................... int1 s;
983 .................... #ifdef _ERRNO 946 .................... #ifdef _ERRNO
984 .................... if(x > 709.7827128) 947 .................... if(x > 709.7827128)
985 .................... { 948 .................... {
986 .................... errno=ERANGE; 949 .................... errno=ERANGE;
987 .................... return(0); 950 .................... return(0);
988 .................... } 951 .................... }
989 .................... #endif 952 .................... #endif
990 .................... n = (signed int16)(x/LN2); 953 .................... n = (signed int16)(x/LN2);
991 .................... s = 0; 954 .................... s = 0;
992 .................... y = x; 955 .................... y = x;
993 .................... 956 ....................
994 .................... if (x < 0) 957 .................... if (x < 0)
995 .................... { 958 .................... {
996 .................... s = 1; 959 .................... s = 1;
997 .................... n = -n; 960 .................... n = -n;
998 .................... y = -y; 961 .................... y = -y;
999 .................... } 962 .................... }
1000 .................... 963 ....................
1001 .................... res = 0.0; 964 .................... res = 0.0;
1002 .................... 965 ....................
1003 .................... #if !defined(__PCD__) 966 .................... #if !defined(__PCD__)
1004 .................... *((unsigned int16 *)(&res)) = n + 0x7F; 967 .................... *((unsigned int16 *)(&res)) = n + 0x7F;
1005 .................... #endif 968 .................... #endif
1006 .................... p= (((unsigned int16 *)(&res))+3); 969 .................... p= (((unsigned int16 *)(&res))+3);
1007 .................... data1 = *p; 970 .................... data1 = *p;
1008 .................... data2 = *p; 971 .................... data2 = *p;
1009 .................... data1 = n + 0x3FF; 972 .................... data1 = n + 0x3FF;
1010 .................... data1 = data1 <<4; 973 .................... data1 = data1 <<4;
1011 .................... if(bit_test(data2,15)) 974 .................... if(bit_test(data2,15))
1012 .................... bit_set(data1,15); 975 .................... bit_set(data1,15);
1013 .................... data2 = data2 & 0x000F; 976 .................... data2 = data2 & 0x000F;
1014 .................... data1 ^= data2; 977 .................... data1 ^= data2;
1015 .................... 978 ....................
1016 .................... *(((unsigned int16 *)(&res)+3)) = data1; 979 .................... *(((unsigned int16 *)(&res)+3)) = data1;
1017 .................... 980 ....................
1018 .................... 981 ....................
1019 .................... y = y/LN2 - (float64)n; 982 .................... y = y/LN2 - (float64)n;
1020 .................... 983 ....................
1021 .................... r = pe[0]*y + pe[1]; 984 .................... r = pe[0]*y + pe[1];
1022 .................... r = r*y + pe[2]; 985 .................... r = r*y + pe[2];
1023 .................... r = r*y + pe[3]; 986 .................... r = r*y + pe[3];
1024 .................... r = r*y + pe[4]; 987 .................... r = r*y + pe[4];
1025 .................... r = r*y + pe[5]; 988 .................... r = r*y + pe[5];
1026 .................... 989 ....................
1027 .................... res = res*(1.0 + y*r); 990 .................... res = res*(1.0 + y*r);
1028 .................... 991 ....................
1029 .................... if (s) 992 .................... if (s)
1030 .................... res = 1.0/res; 993 .................... res = 1.0/res;
1031 .................... return(res); 994 .................... return(res);
1032 .................... } 995 .................... }
1033 .................... 996 ....................
1034 .................... #ENDIF 997 .................... #ENDIF
1035 .................... 998 ....................
1036 .................... 999 ....................
1037 .................... /************************************************************/ 1000 .................... /************************************************************/
1038 .................... 1001 ....................
1039 .................... float32 const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 1002 .................... float32 const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189};
1040 .................... float32 const ql[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; 1003 .................... float32 const ql[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943};
1041 .................... 1004 ....................
1042 .................... //////////////////////////////////////////////////////////////////////////// 1005 .................... ////////////////////////////////////////////////////////////////////////////
1043 .................... // float log(float x) 1006 .................... // float log(float x)
1044 .................... //////////////////////////////////////////////////////////////////////////// 1007 .................... ////////////////////////////////////////////////////////////////////////////
1045 .................... // Description : returns the the natural log of x 1008 .................... // Description : returns the the natural log of x
1046 .................... // Date : N/A 1009 .................... // Date : N/A
1047 .................... // 1010 .................... //
1048 .................... float32 log(float32 x) 1011 .................... float32 log(float32 x)
1049 .................... { 1012 .................... {
1050 .................... float32 y, res, r, y2; 1013 .................... float32 y, res, r, y2;
1051 .................... #if defined(__PCD__) 1014 .................... #if defined(__PCD__)
1052 .................... unsigned int8 data1,data2; 1015 .................... unsigned int8 data1,data2;
1053 .................... #endif 1016 .................... #endif
1054 .................... signed int8 n; 1017 .................... signed int8 n;
1055 .................... #ifdef _ERRNO 1018 .................... #ifdef _ERRNO
1056 .................... if(x <0) 1019 .................... if(x <0)
1057 .................... { 1020 .................... {
1058 .................... errno=EDOM; 1021 .................... errno=EDOM;
1059 .................... } 1022 .................... }
1060 .................... if(x ==0) 1023 .................... if(x ==0)
1061 .................... { 1024 .................... {
1062 .................... errno=ERANGE; 1025 .................... errno=ERANGE;
1063 .................... return(0); 1026 .................... return(0);
1064 .................... } 1027 .................... }
1065 .................... #endif 1028 .................... #endif
1066 .................... y = x; 1029 .................... y = x;
1067 .................... 1030 ....................
1068 .................... if (y != 1.0) 1031 .................... if (y != 1.0)
1069 .................... { 1032 .................... {
1070 .................... #if !defined(__PCD__) 1033 .................... #if !defined(__PCD__)
1071 .................... *((unsigned int8 *)(&y)) = 0x7E; 1034 .................... *((unsigned int8 *)(&y)) = 0x7E;
1072 .................... #endif 1035 .................... #endif
1073 .................... 1036 ....................
1074 .................... #if defined(__PCD__) // Takes care of IEEE format 1037 .................... #if defined(__PCD__) // Takes care of IEEE format
1075 .................... data2 = *(((unsigned int8 *)(&y))+3); 1038 .................... data2 = *(((unsigned int8 *)(&y))+3);
1076 .................... *(((unsigned int8 *)(&y))+3) = 0x3F; 1039 .................... *(((unsigned int8 *)(&y))+3) = 0x3F;
1077 .................... data1 = *(((unsigned int8 *)(&y))+2); 1040 .................... data1 = *(((unsigned int8 *)(&y))+2);
1078 .................... bit_clear(data1,7); 1041 .................... bit_clear(data1,7);
1079 .................... *(((unsigned int8 *)(&y))+2) = data1; 1042 .................... *(((unsigned int8 *)(&y))+2) = data1;
1080 .................... if(bit_test(data2,7)) 1043 .................... if(bit_test(data2,7))
1081 .................... bit_set(*(((unsigned int8 *)(&y))+3),7); 1044 .................... bit_set(*(((unsigned int8 *)(&y))+3),7);
1082 .................... #endif 1045 .................... #endif
1083 .................... 1046 ....................
1084 .................... y = (y - 1.0)/(y + 1.0); 1047 .................... y = (y - 1.0)/(y + 1.0);
1085 .................... 1048 ....................
1086 .................... y2=y*y; 1049 .................... y2=y*y;
1087 .................... 1050 ....................
1088 .................... res = pl[0]*y2 + pl[1]; 1051 .................... res = pl[0]*y2 + pl[1];
1089 .................... res = res*y2 + pl[2]; 1052 .................... res = res*y2 + pl[2];
1090 .................... res = res*y2 + pl[3]; 1053 .................... res = res*y2 + pl[3];
1091 .................... 1054 ....................
1092 .................... r = ql[0]*y2 + ql[1]; 1055 .................... r = ql[0]*y2 + ql[1];
1093 .................... r = r*y2 + ql[2]; 1056 .................... r = r*y2 + ql[2];
1094 .................... r = r*y2 + ql[3]; 1057 .................... r = r*y2 + ql[3];
1095 .................... 1058 ....................
1096 .................... res = y*res/r; 1059 .................... res = y*res/r;
1097 .................... #if !defined(__PCD__) 1060 .................... #if !defined(__PCD__)
1098 .................... n = *((unsigned int8 *)(&x)) - 0x7E; 1061 .................... n = *((unsigned int8 *)(&x)) - 0x7E;
1099 .................... #endif 1062 .................... #endif
1100 .................... #if defined(__PCD__) 1063 .................... #if defined(__PCD__)
1101 .................... data1 = *(((unsigned int8 *)(&x)+3)); 1064 .................... data1 = *(((unsigned int8 *)(&x)+3));
1102 .................... rotate_left(&data1,1); 1065 .................... rotate_left(&data1,1);
1103 .................... data2 = *(((unsigned int8 *)(&x)+2)); 1066 .................... data2 = *(((unsigned int8 *)(&x)+2));
1104 .................... if(bit_test (data2,7)) 1067 .................... if(bit_test (data2,7))
1105 .................... bit_set(data1,0); 1068 .................... bit_set(data1,0);
1106 .................... n = data1 - 0x7E; 1069 .................... n = data1 - 0x7E;
1107 .................... #endif 1070 .................... #endif
1108 .................... 1071 ....................
1109 .................... if (n<0) 1072 .................... if (n<0)
1110 .................... r = -(float32)-n; 1073 .................... r = -(float32)-n;
1111 .................... else 1074 .................... else
1112 .................... r = (float32)n; 1075 .................... r = (float32)n;
1113 .................... 1076 ....................
1114 .................... res += r*LN2; 1077 .................... res += r*LN2;
1115 .................... } 1078 .................... }
1116 .................... 1079 ....................
1117 .................... else 1080 .................... else
1118 .................... res = 0.0; 1081 .................... res = 0.0;
1119 .................... 1082 ....................
1120 .................... return(res); 1083 .................... return(res);
1121 .................... } 1084 .................... }
1122 .................... 1085 ....................
1123 .................... //Overloaded function for log() for PCD 1086 .................... //Overloaded function for log() for PCD
1124 .................... // Overloaded function log() for data type - Float48 1087 .................... // Overloaded function log() for data type - Float48
1125 .................... #if defined(__PCD__) 1088 .................... #if defined(__PCD__)
1126 .................... float48 log(float48 x) 1089 .................... float48 log(float48 x)
1127 .................... { 1090 .................... {
1128 .................... float48 y, res, r, y2; 1091 .................... float48 y, res, r, y2;
1129 .................... unsigned int8 data1,data2; 1092 .................... unsigned int8 data1,data2;
1130 .................... signed int8 n; 1093 .................... signed int8 n;
1131 .................... #ifdef _ERRNO 1094 .................... #ifdef _ERRNO
1132 .................... if(x <0) 1095 .................... if(x <0)
1133 .................... { 1096 .................... {
1134 .................... errno=EDOM; 1097 .................... errno=EDOM;
1135 .................... } 1098 .................... }
1136 .................... if(x ==0) 1099 .................... if(x ==0)
1137 .................... { 1100 .................... {
1138 .................... errno=ERANGE; 1101 .................... errno=ERANGE;
1139 .................... return(0); 1102 .................... return(0);
1140 .................... } 1103 .................... }
1141 .................... #endif 1104 .................... #endif
1142 .................... y = x; 1105 .................... y = x;
1143 .................... 1106 ....................
1144 .................... if (y != 1.0) 1107 .................... if (y != 1.0)
1145 .................... { 1108 .................... {
1146 .................... 1109 ....................
1147 .................... #if !defined(__PCD__) 1110 .................... #if !defined(__PCD__)
1148 .................... *((unsigned int8 *)(&y)) = 0x7E; 1111 .................... *((unsigned int8 *)(&y)) = 0x7E;
1149 .................... #endif 1112 .................... #endif
1150 .................... data2 = *(((unsigned int8 *)(&y))+5); 1113 .................... data2 = *(((unsigned int8 *)(&y))+5);
1151 .................... *(((unsigned int8 *)(&y))+5) = 0x3F; 1114 .................... *(((unsigned int8 *)(&y))+5) = 0x3F;
1152 .................... data1 = *(((unsigned int8 *)(&y))+4); 1115 .................... data1 = *(((unsigned int8 *)(&y))+4);
1153 .................... bit_clear(data1,7); 1116 .................... bit_clear(data1,7);
1154 .................... *(((unsigned int8 *)(&y))+4) = data1; 1117 .................... *(((unsigned int8 *)(&y))+4) = data1;
1155 .................... 1118 ....................
1156 .................... if(bit_test(data2,7)) 1119 .................... if(bit_test(data2,7))
1157 .................... bit_set(*(((unsigned int8 *)(&y))+4),7); 1120 .................... bit_set(*(((unsigned int8 *)(&y))+4),7);
1158 .................... y = (y - 1.0)/(y + 1.0); 1121 .................... y = (y - 1.0)/(y + 1.0);
1159 .................... 1122 ....................
1160 .................... y2=y*y; 1123 .................... y2=y*y;
1161 .................... 1124 ....................
1162 .................... res = pl[0]*y2 + pl[1]; 1125 .................... res = pl[0]*y2 + pl[1];
1163 .................... res = res*y2 + pl[2]; 1126 .................... res = res*y2 + pl[2];
1164 .................... res = res*y2 + pl[3]; 1127 .................... res = res*y2 + pl[3];
1165 .................... 1128 ....................
1166 .................... r = ql[0]*y2 + ql[1]; 1129 .................... r = ql[0]*y2 + ql[1];
1167 .................... r = r*y2 + ql[2]; 1130 .................... r = r*y2 + ql[2];
1168 .................... r = r*y2 + ql[3]; 1131 .................... r = r*y2 + ql[3];
1169 .................... 1132 ....................
1170 .................... res = y*res/r; 1133 .................... res = y*res/r;
1171 .................... 1134 ....................
1172 .................... data1 = *(((unsigned int8 *)(&x)+5)); 1135 .................... data1 = *(((unsigned int8 *)(&x)+5));
1173 .................... rotate_left(&data1,1); 1136 .................... rotate_left(&data1,1);
1174 .................... data2 = *(((unsigned int8 *)(&x)+4)); 1137 .................... data2 = *(((unsigned int8 *)(&x)+4));
1175 .................... if(bit_test (data2,7)) 1138 .................... if(bit_test (data2,7))
1176 .................... bit_set(data1,0); 1139 .................... bit_set(data1,0);
1177 .................... 1140 ....................
1178 .................... n = data1 - 0x7E; 1141 .................... n = data1 - 0x7E;
1179 .................... 1142 ....................
1180 .................... if (n<0) 1143 .................... if (n<0)
1181 .................... r = -(float48)-n; 1144 .................... r = -(float48)-n;
1182 .................... else 1145 .................... else
1183 .................... r = (float48)n; 1146 .................... r = (float48)n;
1184 .................... 1147 ....................
1185 .................... res += r*LN2; 1148 .................... res += r*LN2;
1186 .................... } 1149 .................... }
1187 .................... 1150 ....................
1188 .................... else 1151 .................... else
1189 .................... res = 0.0; 1152 .................... res = 0.0;
1190 .................... 1153 ....................
1191 .................... return(res); 1154 .................... return(res);
1192 .................... } 1155 .................... }
1193 .................... 1156 ....................
1194 .................... // Overloaded function log() for data type - Float48 1157 .................... // Overloaded function log() for data type - Float48
1195 .................... #if defined(__PCD__) 1158 .................... #if defined(__PCD__)
1196 .................... float32 const pl_64[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 1159 .................... float32 const pl_64[4] = {0.45145214, -9.0558803, 26.940971, -19.860189};
1197 .................... float32 const ql_64[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; 1160 .................... float32 const ql_64[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943};
1198 .................... #endif 1161 .................... #endif
1199 .................... float64 log(float64 x) 1162 .................... float64 log(float64 x)
1200 .................... { 1163 .................... {
1201 .................... float64 y, res, r, y2; 1164 .................... float64 y, res, r, y2;
1202 .................... unsigned int16 data1,data2; 1165 .................... unsigned int16 data1,data2;
1203 .................... unsigned int16 *p; 1166 .................... unsigned int16 *p;
1204 .................... signed int16 n; 1167 .................... signed int16 n;
1205 .................... #ifdef _ERRNO 1168 .................... #ifdef _ERRNO
1206 .................... if(x <0) 1169 .................... if(x <0)
1207 .................... { 1170 .................... {
1208 .................... errno=EDOM; 1171 .................... errno=EDOM;
1209 .................... } 1172 .................... }
1210 .................... if(x ==0) 1173 .................... if(x ==0)
1211 .................... { 1174 .................... {
1212 .................... errno=ERANGE; 1175 .................... errno=ERANGE;
1213 .................... return(0); 1176 .................... return(0);
1214 .................... } 1177 .................... }
1215 .................... #endif 1178 .................... #endif
1216 .................... y = x; 1179 .................... y = x;
1217 .................... 1180 ....................
1218 .................... if (y != 1.0) 1181 .................... if (y != 1.0)
1219 .................... { 1182 .................... {
1220 .................... #if !defined(__PCD__) 1183 .................... #if !defined(__PCD__)
1221 .................... *((unsigned int8 *)(&y)) = 0x7E; 1184 .................... *((unsigned int8 *)(&y)) = 0x7E;
1222 .................... #endif 1185 .................... #endif
1223 .................... p= (((unsigned int16 *)(&y))+3); 1186 .................... p= (((unsigned int16 *)(&y))+3);
1224 .................... data1 = *p; 1187 .................... data1 = *p;
1225 .................... data2 = *p; 1188 .................... data2 = *p;
1226 .................... data1 = 0x3FE; 1189 .................... data1 = 0x3FE;
1227 .................... data1 = data1 <<4; 1190 .................... data1 = data1 <<4;
1228 .................... if(bit_test (data2,15)) 1191 .................... if(bit_test (data2,15))
1229 .................... bit_set(data1,15); 1192 .................... bit_set(data1,15);
1230 .................... data2 = data2 & 0x000F; 1193 .................... data2 = data2 & 0x000F;
1231 .................... data1 ^=data2; 1194 .................... data1 ^=data2;
1232 .................... 1195 ....................
1233 .................... *p = data1; 1196 .................... *p = data1;
1234 .................... 1197 ....................
1235 .................... y = (y - 1.0)/(y + 1.0); 1198 .................... y = (y - 1.0)/(y + 1.0);
1236 .................... 1199 ....................
1237 .................... y2=y*y; 1200 .................... y2=y*y;
1238 .................... 1201 ....................
1239 .................... res = pl_64[0]*y2 + pl_64[1]; 1202 .................... res = pl_64[0]*y2 + pl_64[1];
1240 .................... res = res*y2 + pl_64[2]; 1203 .................... res = res*y2 + pl_64[2];
1241 .................... res = res*y2 + pl_64[3]; 1204 .................... res = res*y2 + pl_64[3];
1242 .................... 1205 ....................
1243 .................... r = ql_64[0]*y2 + ql_64[1]; 1206 .................... r = ql_64[0]*y2 + ql_64[1];
1244 .................... r = r*y2 + ql_64[2]; 1207 .................... r = r*y2 + ql_64[2];
1245 .................... r = r*y2 + ql_64[3]; 1208 .................... r = r*y2 + ql_64[3];
1246 .................... 1209 ....................
1247 .................... res = y*res/r; 1210 .................... res = y*res/r;
1248 .................... 1211 ....................
1249 .................... p= (((unsigned int16 *)(&x))+3); 1212 .................... p= (((unsigned int16 *)(&x))+3);
1250 .................... data1 = *p; 1213 .................... data1 = *p;
1251 .................... bit_clear(data1,15); 1214 .................... bit_clear(data1,15);
1252 .................... data1 = data1 >>4; 1215 .................... data1 = data1 >>4;
1253 .................... n = data1 - 0x3FE; 1216 .................... n = data1 - 0x3FE;
1254 .................... 1217 ....................
1255 .................... 1218 ....................
1256 .................... if (n<0) 1219 .................... if (n<0)
1257 .................... r = -(float64)-n; 1220 .................... r = -(float64)-n;
1258 .................... else 1221 .................... else
1259 .................... r = (float64)n; 1222 .................... r = (float64)n;
1260 .................... 1223 ....................
1261 .................... res += r*LN2; 1224 .................... res += r*LN2;
1262 .................... } 1225 .................... }
1263 .................... 1226 ....................
1264 .................... else 1227 .................... else
1265 .................... res = 0.0; 1228 .................... res = 0.0;
1266 .................... 1229 ....................
1267 .................... return(res); 1230 .................... return(res);
1268 .................... } 1231 .................... }
1269 .................... #endif 1232 .................... #endif
1270 .................... 1233 ....................
1271 .................... 1234 ....................
1272 .................... #define LN10 2.3025850929940456 1235 .................... #define LN10 2.3025850929940456
1273 .................... 1236 ....................
1274 .................... //////////////////////////////////////////////////////////////////////////// 1237 .................... ////////////////////////////////////////////////////////////////////////////
1275 .................... // float log10(float x) 1238 .................... // float log10(float x)
1276 .................... //////////////////////////////////////////////////////////////////////////// 1239 .................... ////////////////////////////////////////////////////////////////////////////
1277 .................... // Description : returns the the log base 10 of x 1240 .................... // Description : returns the the log base 10 of x
1278 .................... // Date : N/A 1241 .................... // Date : N/A
1279 .................... // 1242 .................... //
1280 .................... float32 log10(float32 x) 1243 .................... float32 log10(float32 x)
1281 .................... { 1244 .................... {
1282 .................... float32 r; 1245 .................... float32 r;
1283 .................... 1246 ....................
1284 .................... r = log(x); 1247 .................... r = log(x);
1285 .................... r = r/LN10; 1248 .................... r = r/LN10;
1286 .................... return(r); 1249 .................... return(r);
1287 .................... } 1250 .................... }
1288 .................... 1251 ....................
1289 .................... //Overloaded functions for log10() for PCD 1252 .................... //Overloaded functions for log10() for PCD
1290 .................... // Overloaded function log10() for data type - Float48 1253 .................... // Overloaded function log10() for data type - Float48
1291 .................... #if defined(__PCD__) 1254 .................... #if defined(__PCD__)
1292 .................... float48 log10(float48 x) 1255 .................... float48 log10(float48 x)
1293 .................... { 1256 .................... {
1294 .................... float48 r; 1257 .................... float48 r;
1295 .................... 1258 ....................
1296 .................... r = log(x); 1259 .................... r = log(x);
1297 .................... r = r/LN10; 1260 .................... r = r/LN10;
1298 .................... return(r); 1261 .................... return(r);
1299 .................... } 1262 .................... }
1300 .................... 1263 ....................
1301 .................... // Overloaded function log10() for data type - Float64 1264 .................... // Overloaded function log10() for data type - Float64
1302 .................... float64 log10(float64 x) 1265 .................... float64 log10(float64 x)
1303 .................... { 1266 .................... {
1304 .................... float64 r; 1267 .................... float64 r;
1305 .................... 1268 ....................
1306 .................... r = log(x); 1269 .................... r = log(x);
1307 .................... r = r/LN10; 1270 .................... r = r/LN10;
1308 .................... return(r); 1271 .................... return(r);
1309 .................... } 1272 .................... }
1310 .................... #endif 1273 .................... #endif
1311 .................... //////////////////////////////////////////////////////////////////////////// 1274 .................... ////////////////////////////////////////////////////////////////////////////
1312 .................... // float modf(float x) 1275 .................... // float modf(float x)
1313 .................... //////////////////////////////////////////////////////////////////////////// 1276 .................... ////////////////////////////////////////////////////////////////////////////
1314 .................... // Description :breaks the argument value int integral and fractional parts, 1277 .................... // Description :breaks the argument value int integral and fractional parts,
1315 .................... // ach of which have the same sign as the argument. It stores the integral part 1278 .................... // ach of which have the same sign as the argument. It stores the integral part
1316 .................... // as a float in the object pointed to by the iptr 1279 .................... // as a float in the object pointed to by the iptr
1317 .................... // Returns : returns the signed fractional part of value. 1280 .................... // Returns : returns the signed fractional part of value.
1318 .................... // Date : N/A 1281 .................... // Date : N/A
1319 .................... // 1282 .................... //
1320 .................... 1283 ....................
1321 .................... float32 modf(float32 value,float32 *iptr) 1284 .................... float32 modf(float32 value,float32 *iptr)
1322 .................... { 1285 .................... {
1323 .................... *iptr=(value < 0.0)? ceil(value): floor(value); 1286 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1324 .................... return(value - *iptr); 1287 .................... return(value - *iptr);
1325 .................... } 1288 .................... }
1326 .................... //Overloaded functions for modf() for PCD 1289 .................... //Overloaded functions for modf() for PCD
1327 .................... // Overloaded function modf() for data type - Float48 1290 .................... // Overloaded function modf() for data type - Float48
1328 .................... #if defined(__PCD__) 1291 .................... #if defined(__PCD__)
1329 .................... float48 modf(float48 value,float48 *iptr) 1292 .................... float48 modf(float48 value,float48 *iptr)
1330 .................... { 1293 .................... {
1331 .................... *iptr=(value < 0.0)? ceil(value): floor(value); 1294 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1332 .................... return(value - *iptr); 1295 .................... return(value - *iptr);
1333 .................... } 1296 .................... }
1334 .................... // Overloaded function modf() for data type - Float64 1297 .................... // Overloaded function modf() for data type - Float64
1335 .................... float64 modf(float64 value,float64 *iptr) 1298 .................... float64 modf(float64 value,float64 *iptr)
1336 .................... { 1299 .................... {
1337 .................... *iptr=(value < 0.0)? ceil(value): floor(value); 1300 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1338 .................... return(value - *iptr); 1301 .................... return(value - *iptr);
1339 .................... } 1302 .................... }
1340 .................... #endif 1303 .................... #endif
1341 .................... 1304 ....................
1342 .................... //////////////////////////////////////////////////////////////////////////// 1305 .................... ////////////////////////////////////////////////////////////////////////////
1343 .................... // float pwr(float x,float y) 1306 .................... // float pwr(float x,float y)
1344 .................... //////////////////////////////////////////////////////////////////////////// 1307 .................... ////////////////////////////////////////////////////////////////////////////
1345 .................... // Description : returns the value (x^y) 1308 .................... // Description : returns the value (x^y)
1346 .................... // Date : N/A 1309 .................... // Date : N/A
1347 .................... // Note : 0 is returned when the function will generate an imaginary number 1310 .................... // Note : 0 is returned when the function will generate an imaginary number
1348 .................... // 1311 .................... //
1349 .................... float32 pwr(float32 x,float32 y) 1312 .................... float32 pwr(float32 x,float32 y)
1350 .................... { 1313 .................... {
1351 .................... if(0 > x && fmod(y, 1) == 0) { 1314 .................... if(0 > x && fmod(y, 1) == 0) {
1352 .................... if(fmod(y, 2) == 0) { 1315 .................... if(fmod(y, 2) == 0) {
1353 .................... return (exp(log(-x) * y)); 1316 .................... return (exp(log(-x) * y));
1354 .................... } else { 1317 .................... } else {
1355 .................... return (-exp(log(-x) * y)); 1318 .................... return (-exp(log(-x) * y));
1356 .................... } 1319 .................... }
1357 .................... } else if(0 > x && fmod(y, 1) != 0) { 1320 .................... } else if(0 > x && fmod(y, 1) != 0) {
1358 .................... return 0; 1321 .................... return 0;
1359 .................... } else { 1322 .................... } else {
1360 .................... if(x != 0 || 0 >= y) { 1323 .................... if(x != 0 || 0 >= y) {
1361 .................... return (exp(log(x) * y)); 1324 .................... return (exp(log(x) * y));
1362 .................... } 1325 .................... }
1363 .................... } 1326 .................... }
1364 .................... } 1327 .................... }
1365 .................... //Overloaded functions for pwr() for PCD 1328 .................... //Overloaded functions for pwr() for PCD
1366 .................... // Overloaded function pwr() for data type - Float48 1329 .................... // Overloaded function pwr() for data type - Float48
1367 .................... #if defined(__PCD__) 1330 .................... #if defined(__PCD__)
1368 .................... float48 pwr(float48 x,float48 y) 1331 .................... float48 pwr(float48 x,float48 y)
1369 .................... { 1332 .................... {
1370 .................... if(0 > x && fmod(y, 1) == 0) { 1333 .................... if(0 > x && fmod(y, 1) == 0) {
1371 .................... if(fmod(y, 2) == 0) { 1334 .................... if(fmod(y, 2) == 0) {
1372 .................... return (exp(log(-x) * y)); 1335 .................... return (exp(log(-x) * y));
1373 .................... } else { 1336 .................... } else {
1374 .................... return (-exp(log(-x) * y)); 1337 .................... return (-exp(log(-x) * y));
1375 .................... } 1338 .................... }
1376 .................... } else if(0 > x && fmod(y, 1) != 0) { 1339 .................... } else if(0 > x && fmod(y, 1) != 0) {
1377 .................... return 0; 1340 .................... return 0;
1378 .................... } else { 1341 .................... } else {
1379 .................... if(x != 0 || 0 >= y) { 1342 .................... if(x != 0 || 0 >= y) {
1380 .................... return (exp(log(x) * y)); 1343 .................... return (exp(log(x) * y));
1381 .................... } 1344 .................... }
1382 .................... } 1345 .................... }
1383 .................... } 1346 .................... }
1384 .................... // Overloaded function pwr() for data type - Float64 1347 .................... // Overloaded function pwr() for data type - Float64
1385 .................... float64 pwr(float64 x,float64 y) 1348 .................... float64 pwr(float64 x,float64 y)
1386 .................... { 1349 .................... {
1387 .................... if(0 > x && fmod(y, 1) == 0) { 1350 .................... if(0 > x && fmod(y, 1) == 0) {
1388 .................... if(fmod(y, 2) == 0) { 1351 .................... if(fmod(y, 2) == 0) {
1389 .................... return (exp(log(-x) * y)); 1352 .................... return (exp(log(-x) * y));
1390 .................... } else { 1353 .................... } else {
1391 .................... return (-exp(log(-x) * y)); 1354 .................... return (-exp(log(-x) * y));
1392 .................... } 1355 .................... }
1393 .................... } else if(0 > x && fmod(y, 1) != 0) { 1356 .................... } else if(0 > x && fmod(y, 1) != 0) {
1394 .................... return 0; 1357 .................... return 0;
1395 .................... } else { 1358 .................... } else {
1396 .................... if(x != 0 || 0 >= y) { 1359 .................... if(x != 0 || 0 >= y) {
1397 .................... return (exp(log(x) * y)); 1360 .................... return (exp(log(x) * y));
1398 .................... } 1361 .................... }
1399 .................... } 1362 .................... }
1400 .................... } 1363 .................... }
1401 .................... #endif 1364 .................... #endif
1402 .................... 1365 ....................
1403 .................... //////////////////// Power functions //////////////////// 1366 .................... //////////////////// Power functions ////////////////////
1404 .................... 1367 ....................
1405 .................... //////////////////////////////////////////////////////////////////////////// 1368 .................... ////////////////////////////////////////////////////////////////////////////
1406 .................... // float pow(float x,float y) 1369 .................... // float pow(float x,float y)
1407 .................... //////////////////////////////////////////////////////////////////////////// 1370 .................... ////////////////////////////////////////////////////////////////////////////
1408 .................... // Description : returns the value (x^y) 1371 .................... // Description : returns the value (x^y)
1409 .................... // Date : N/A 1372 .................... // Date : N/A
1410 .................... // Note : 0 is returned when the function will generate an imaginary number 1373 .................... // Note : 0 is returned when the function will generate an imaginary number
1411 .................... // 1374 .................... //
1412 .................... float32 pow(float32 x,float32 y) 1375 .................... float32 pow(float32 x,float32 y)
1413 .................... { 1376 .................... {
1414 .................... if(0 > x && fmod(y, 1) == 0) { 1377 .................... if(0 > x && fmod(y, 1) == 0) {
1415 .................... if(fmod(y, 2) == 0) { 1378 .................... if(fmod(y, 2) == 0) {
1416 .................... return (exp(log(-x) * y)); 1379 .................... return (exp(log(-x) * y));
1417 .................... } else { 1380 .................... } else {
1418 .................... return (-exp(log(-x) * y)); 1381 .................... return (-exp(log(-x) * y));
1419 .................... } 1382 .................... }
1420 .................... } else if(0 > x && fmod(y, 1) != 0) { 1383 .................... } else if(0 > x && fmod(y, 1) != 0) {
1421 .................... return 0; 1384 .................... return 0;
1422 .................... } else { 1385 .................... } else {
1423 .................... if(x != 0 || 0 >= y) { 1386 .................... if(x != 0 || 0 >= y) {
1424 .................... return (exp(log(x) * y)); 1387 .................... return (exp(log(x) * y));
1425 .................... } 1388 .................... }
1426 .................... } 1389 .................... }
1427 .................... } 1390 .................... }
1428 .................... //Overloaded functions for pow() for PCD 1391 .................... //Overloaded functions for pow() for PCD
1429 .................... // Overloaded function for pow() data type - Float48 1392 .................... // Overloaded function for pow() data type - Float48
1430 .................... #if defined(__PCD__) 1393 .................... #if defined(__PCD__)
1431 .................... float48 pow(float48 x,float48 y) 1394 .................... float48 pow(float48 x,float48 y)
1432 .................... { 1395 .................... {
1433 .................... if(0 > x && fmod(y, 1) == 0) { 1396 .................... if(0 > x && fmod(y, 1) == 0) {
1434 .................... if(fmod(y, 2) == 0) { 1397 .................... if(fmod(y, 2) == 0) {
1435 .................... return (exp(log(-x) * y)); 1398 .................... return (exp(log(-x) * y));
1436 .................... } else { 1399 .................... } else {
1437 .................... return (-exp(log(-x) * y)); 1400 .................... return (-exp(log(-x) * y));
1438 .................... } 1401 .................... }
1439 .................... } else if(0 > x && fmod(y, 1) != 0) { 1402 .................... } else if(0 > x && fmod(y, 1) != 0) {
1440 .................... return 0; 1403 .................... return 0;
1441 .................... } else { 1404 .................... } else {
1442 .................... if(x != 0 || 0 >= y) { 1405 .................... if(x != 0 || 0 >= y) {
1443 .................... return (exp(log(x) * y)); 1406 .................... return (exp(log(x) * y));
1444 .................... } 1407 .................... }
1445 .................... } 1408 .................... }
1446 .................... } 1409 .................... }
1447 .................... 1410 ....................
1448 .................... // Overloaded function pow() for data type - Float64 1411 .................... // Overloaded function pow() for data type - Float64
1449 .................... float64 pow(float64 x,float64 y) 1412 .................... float64 pow(float64 x,float64 y)
1450 .................... { 1413 .................... {
1451 .................... if(0 > x && fmod(y, 1) == 0) { 1414 .................... if(0 > x && fmod(y, 1) == 0) {
1452 .................... if(fmod(y, 2) == 0) { 1415 .................... if(fmod(y, 2) == 0) {
1453 .................... return (exp(log(-x) * y)); 1416 .................... return (exp(log(-x) * y));
1454 .................... } else { 1417 .................... } else {
1455 .................... return (-exp(log(-x) * y)); 1418 .................... return (-exp(log(-x) * y));
1456 .................... } 1419 .................... }
1457 .................... } else if(0 > x && fmod(y, 1) != 0) { 1420 .................... } else if(0 > x && fmod(y, 1) != 0) {
1458 .................... return 0; 1421 .................... return 0;
1459 .................... } else { 1422 .................... } else {
1460 .................... if(x != 0 || 0 >= y) { 1423 .................... if(x != 0 || 0 >= y) {
1461 .................... return (exp(log(x) * y)); 1424 .................... return (exp(log(x) * y));
1462 .................... } 1425 .................... }
1463 .................... } 1426 .................... }
1464 .................... } 1427 .................... }
1465 .................... #endif 1428 .................... #endif
1466 .................... 1429 ....................
1467 .................... //////////////////////////////////////////////////////////////////////////// 1430 .................... ////////////////////////////////////////////////////////////////////////////
1468 .................... // float sqrt(float x) 1431 .................... // float sqrt(float x)
1469 .................... //////////////////////////////////////////////////////////////////////////// 1432 .................... ////////////////////////////////////////////////////////////////////////////
1470 .................... // Description : returns the square root of x 1433 .................... // Description : returns the square root of x
1471 .................... // Date : N/A 1434 .................... // Date : N/A
1472 .................... // 1435 .................... //
1473 .................... float32 sqrt(float32 x) 1436 .................... float32 sqrt(float32 x)
1474 .................... { 1437 .................... {
1475 .................... float32 y, res; 1438 .................... float32 y, res;
1476 .................... #if defined(__PCD__) 1439 .................... #if defined(__PCD__)
1477 .................... unsigned int16 data1,data2; 1440 .................... unsigned int16 data1,data2;
1478 .................... #endif 1441 .................... #endif
1479 .................... BYTE *p; 1442 .................... BYTE *p;
1480 .................... 1443 ....................
1481 .................... #ifdef _ERRNO 1444 .................... #ifdef _ERRNO
1482 .................... if(x < 0) 1445 .................... if(x < 0)
1483 .................... { 1446 .................... {
1484 .................... errno=EDOM; 1447 .................... errno=EDOM;
1485 .................... } 1448 .................... }
1486 .................... #endif 1449 .................... #endif
1487 .................... 1450 ....................
1488 .................... if( x<=0.0) 1451 .................... if( x<=0.0)
1489 .................... return(0.0); 1452 .................... return(0.0);
1490 .................... 1453 ....................
1491 .................... y=x; 1454 .................... y=x;
1492 .................... 1455 ....................
1493 .................... #if !defined(__PCD__) 1456 .................... #if !defined(__PCD__)
1494 .................... p=&y; 1457 .................... p=&y;
1495 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); 1458 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1);
1496 .................... #endif 1459 .................... #endif
1497 .................... 1460 ....................
1498 .................... #if defined(__PCD__) 1461 .................... #if defined(__PCD__)
1499 .................... p = (((unsigned int8 *)(&y))+3); 1462 .................... p = (((unsigned int8 *)(&y))+3);
1500 .................... data1 = *(((unsigned int8 *)(&y))+3); 1463 .................... data1 = *(((unsigned int8 *)(&y))+3);
1501 .................... data2 = *(((unsigned int8 *)(&y))+2); 1464 .................... data2 = *(((unsigned int8 *)(&y))+2);
1502 .................... rotate_left(&data1,1); 1465 .................... rotate_left(&data1,1);
1503 .................... if(bit_test(data2,7)) 1466 .................... if(bit_test(data2,7))
1504 .................... bit_set(data1,0); 1467 .................... bit_set(data1,0);
1505 .................... data1 = ((data1+127) >>1); 1468 .................... data1 = ((data1+127) >>1);
1506 .................... bit_clear(data2,7); 1469 .................... bit_clear(data2,7);
1507 .................... if(bit_test(data1,0)) 1470 .................... if(bit_test(data1,0))
1508 .................... bit_set(data2,7); 1471 .................... bit_set(data2,7);
1509 .................... data1 = data1 >>1; 1472 .................... data1 = data1 >>1;
1510 .................... *(((unsigned int8 *)(&y))+3) = data1; 1473 .................... *(((unsigned int8 *)(&y))+3) = data1;
1511 .................... *(((unsigned int8 *)(&y))+2) = data2; 1474 .................... *(((unsigned int8 *)(&y))+2) = data2;
1512 .................... 1475 ....................
1513 .................... #endif 1476 .................... #endif
1514 .................... 1477 ....................
1515 .................... do { 1478 .................... do {
1516 .................... res=y; 1479 .................... res=y;
1517 .................... y+=(x/y); 1480 .................... y+=(x/y);
1518 .................... 1481 ....................
1519 .................... #if !defined(__PCD__) 1482 .................... #if !defined(__PCD__)
1520 .................... (*p)--; 1483 .................... (*p)--;
1521 .................... #endif 1484 .................... #endif
1522 .................... 1485 ....................
1523 .................... #if defined(__PCD__) 1486 .................... #if defined(__PCD__)
1524 .................... data1 = *(((unsigned int8 *)(&y))+3); 1487 .................... data1 = *(((unsigned int8 *)(&y))+3);
1525 .................... data2 = *(((unsigned int8 *)(&y))+2); 1488 .................... data2 = *(((unsigned int8 *)(&y))+2);
1526 .................... rotate_left(&data1,1); 1489 .................... rotate_left(&data1,1);
1527 .................... if(bit_test(data2,7)) 1490 .................... if(bit_test(data2,7))
1528 .................... bit_set(data1,0); 1491 .................... bit_set(data1,0);
1529 .................... data1--; 1492 .................... data1--;
1530 .................... bit_clear(data2,7); 1493 .................... bit_clear(data2,7);
1531 .................... if(bit_test(data1,0)) 1494 .................... if(bit_test(data1,0))
1532 .................... bit_set(data2,7); 1495 .................... bit_set(data2,7);
1533 .................... data1 = data1 >>1; 1496 .................... data1 = data1 >>1;
1534 .................... *(((unsigned int8 *)(&y))+3) = data1; 1497 .................... *(((unsigned int8 *)(&y))+3) = data1;
1535 .................... *(((unsigned int8 *)(&y))+2) = data2; 1498 .................... *(((unsigned int8 *)(&y))+2) = data2;
1536 .................... 1499 ....................
1537 .................... #endif 1500 .................... #endif
1538 .................... } while(res != y); 1501 .................... } while(res != y);
1539 .................... 1502 ....................
1540 .................... return(res); 1503 .................... return(res);
1541 .................... } 1504 .................... }
1542 .................... //Overloaded functions for sqrt() for PCD 1505 .................... //Overloaded functions for sqrt() for PCD
1543 .................... // Overloaded function sqrt() for data type - Float48 1506 .................... // Overloaded function sqrt() for data type - Float48
1544 .................... #if defined(__PCD__) 1507 .................... #if defined(__PCD__)
1545 .................... float48 sqrt(float48 x) 1508 .................... float48 sqrt(float48 x)
1546 .................... { 1509 .................... {
1547 .................... float48 y, res; 1510 .................... float48 y, res;
1548 .................... unsigned int16 data1,data2; 1511 .................... unsigned int16 data1,data2;
1549 .................... BYTE *p; 1512 .................... BYTE *p;
1550 .................... 1513 ....................
1551 .................... #ifdef _ERRNO 1514 .................... #ifdef _ERRNO
1552 .................... if(x < 0) 1515 .................... if(x < 0)
1553 .................... { 1516 .................... {
1554 .................... errno=EDOM; 1517 .................... errno=EDOM;
1555 .................... } 1518 .................... }
1556 .................... #endif 1519 .................... #endif
1557 .................... 1520 ....................
1558 .................... if( x<=0.0) 1521 .................... if( x<=0.0)
1559 .................... return(0.0); 1522 .................... return(0.0);
1560 .................... 1523 ....................
1561 .................... y=x; 1524 .................... y=x;
1562 .................... 1525 ....................
1563 .................... #if !defined(__PCD__) 1526 .................... #if !defined(__PCD__)
1564 .................... p=&y; 1527 .................... p=&y;
1565 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); 1528 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1);
1566 .................... #endif 1529 .................... #endif
1567 .................... 1530 ....................
1568 .................... #if defined(__PCD__) 1531 .................... #if defined(__PCD__)
1569 .................... p = (((unsigned int8 *)(&y))+5); 1532 .................... p = (((unsigned int8 *)(&y))+5);
1570 .................... data1 = *(((unsigned int8 *)(&y))+5); 1533 .................... data1 = *(((unsigned int8 *)(&y))+5);
1571 .................... data2 = *(((unsigned int8 *)(&y))+4); 1534 .................... data2 = *(((unsigned int8 *)(&y))+4);
1572 .................... rotate_left(&data1,1); 1535 .................... rotate_left(&data1,1);
1573 .................... if(bit_test(data2,7)) 1536 .................... if(bit_test(data2,7))
1574 .................... bit_set(data1,0); 1537 .................... bit_set(data1,0);
1575 .................... data1 = ((data1+127) >>1); 1538 .................... data1 = ((data1+127) >>1);
1576 .................... bit_clear(data2,7); 1539 .................... bit_clear(data2,7);
1577 .................... if(bit_test(data1,0)) 1540 .................... if(bit_test(data1,0))
1578 .................... bit_set(data2,7); 1541 .................... bit_set(data2,7);
1579 .................... data1 = data1 >>1; 1542 .................... data1 = data1 >>1;
1580 .................... *(((unsigned int8 *)(&y))+5) = data1; 1543 .................... *(((unsigned int8 *)(&y))+5) = data1;
1581 .................... *(((unsigned int8 *)(&y))+4) = data2; 1544 .................... *(((unsigned int8 *)(&y))+4) = data2;
1582 .................... 1545 ....................
1583 .................... #endif 1546 .................... #endif
1584 .................... 1547 ....................
1585 .................... do { 1548 .................... do {
1586 .................... res=y; 1549 .................... res=y;
1587 .................... y+=(x/y); 1550 .................... y+=(x/y);
1588 .................... 1551 ....................
1589 .................... #if !defined(__PCD__) 1552 .................... #if !defined(__PCD__)
1590 .................... (*p)--; 1553 .................... (*p)--;
1591 .................... #endif 1554 .................... #endif
1592 .................... 1555 ....................
1593 .................... data1 = *(((unsigned int8 *)(&y))+5); 1556 .................... data1 = *(((unsigned int8 *)(&y))+5);
1594 .................... data2 = *(((unsigned int8 *)(&y))+4); 1557 .................... data2 = *(((unsigned int8 *)(&y))+4);
1595 .................... rotate_left(&data1,1); 1558 .................... rotate_left(&data1,1);
1596 .................... if(bit_test(data2,7)) 1559 .................... if(bit_test(data2,7))
1597 .................... bit_set(data1,0); 1560 .................... bit_set(data1,0);
1598 .................... data1--; 1561 .................... data1--;
1599 .................... bit_clear(data2,7); 1562 .................... bit_clear(data2,7);
1600 .................... if(bit_test(data1,0)) 1563 .................... if(bit_test(data1,0))
1601 .................... bit_set(data2,7); 1564 .................... bit_set(data2,7);
1602 .................... data1 = data1 >>1; 1565 .................... data1 = data1 >>1;
1603 .................... *(((unsigned int8 *)(&y))+5) = data1; 1566 .................... *(((unsigned int8 *)(&y))+5) = data1;
1604 .................... *(((unsigned int8 *)(&y))+4) = data2; 1567 .................... *(((unsigned int8 *)(&y))+4) = data2;
1605 .................... 1568 ....................
1606 .................... } while(res != y); 1569 .................... } while(res != y);
1607 .................... 1570 ....................
1608 .................... return(res); 1571 .................... return(res);
1609 .................... } 1572 .................... }
1610 .................... 1573 ....................
1611 .................... // Overloaded function sqrt() for data type - Float64 1574 .................... // Overloaded function sqrt() for data type - Float64
1612 .................... float64 sqrt(float64 x) 1575 .................... float64 sqrt(float64 x)
1613 .................... { 1576 .................... {
1614 .................... float64 y, res; 1577 .................... float64 y, res;
1615 .................... unsigned int16 *p; 1578 .................... unsigned int16 *p;
1616 .................... unsigned int16 temp1,temp2; 1579 .................... unsigned int16 temp1,temp2;
1617 .................... 1580 ....................
1618 .................... #ifdef _ERRNO 1581 .................... #ifdef _ERRNO
1619 .................... if(x < 0) 1582 .................... if(x < 0)
1620 .................... { 1583 .................... {
1621 .................... errno=EDOM; 1584 .................... errno=EDOM;
1622 .................... } 1585 .................... }
1623 .................... #endif 1586 .................... #endif
1624 .................... 1587 ....................
1625 .................... if( x<=0.0) 1588 .................... if( x<=0.0)
1626 .................... return(0.0); 1589 .................... return(0.0);
1627 .................... 1590 ....................
1628 .................... y=x; 1591 .................... y=x;
1629 .................... p= (((unsigned int16 *)(&y))+3); 1592 .................... p= (((unsigned int16 *)(&y))+3);
1630 .................... temp1 = *p; 1593 .................... temp1 = *p;
1631 .................... temp2 = *p; 1594 .................... temp2 = *p;
1632 .................... bit_clear(temp1,15); 1595 .................... bit_clear(temp1,15);
1633 .................... temp1 = (temp1>>4)+1023; 1596 .................... temp1 = (temp1>>4)+1023;
1634 .................... temp1 = temp1 >> 1; 1597 .................... temp1 = temp1 >> 1;
1635 .................... temp1 = (temp1<<4) & 0xFFF0; 1598 .................... temp1 = (temp1<<4) & 0xFFF0;
1636 .................... if(bit_test(temp2,15)) 1599 .................... if(bit_test(temp2,15))
1637 .................... bit_set(temp1,15); 1600 .................... bit_set(temp1,15);
1638 .................... temp2 = temp2 & 0x000F; 1601 .................... temp2 = temp2 & 0x000F;
1639 .................... temp1 ^= temp2; 1602 .................... temp1 ^= temp2;
1640 .................... 1603 ....................
1641 .................... (*p) = temp1; 1604 .................... (*p) = temp1;
1642 .................... 1605 ....................
1643 .................... do { 1606 .................... do {
1644 .................... res=y; 1607 .................... res=y;
1645 .................... y+=(x/y); 1608 .................... y+=(x/y);
1646 .................... temp1 = *p; 1609 .................... temp1 = *p;
1647 .................... temp2 = *p; 1610 .................... temp2 = *p;
1648 .................... bit_clear(temp1,15); 1611 .................... bit_clear(temp1,15);
1649 .................... temp1 = (temp1>>4); 1612 .................... temp1 = (temp1>>4);
1650 .................... temp1--; 1613 .................... temp1--;
1651 .................... temp1 = (temp1<<4) & 0xFFF0; 1614 .................... temp1 = (temp1<<4) & 0xFFF0;
1652 .................... if(bit_test(temp2,15)) 1615 .................... if(bit_test(temp2,15))
1653 .................... bit_set(temp1,15); 1616 .................... bit_set(temp1,15);
1654 .................... temp2 = temp2 & 0x000F; 1617 .................... temp2 = temp2 & 0x000F;
1655 .................... temp1 ^= temp2; 1618 .................... temp1 ^= temp2;
1656 .................... (*p) = temp1; 1619 .................... (*p) = temp1;
1657 .................... 1620 ....................
1658 .................... } while(res != y); 1621 .................... } while(res != y);
1659 .................... 1622 ....................
1660 .................... return(res); 1623 .................... return(res);
1661 .................... } 1624 .................... }
1662 .................... #endif 1625 .................... #endif
1663 .................... 1626 ....................
1664 .................... ////////////////////////////// Trig Functions ////////////////////////////// 1627 .................... ////////////////////////////// Trig Functions //////////////////////////////
1665 .................... #ifdef PI_DIV_BY_TWO 1628 .................... #ifdef PI_DIV_BY_TWO
1666 .................... #undef PI_DIV_BY_TWO 1629 .................... #undef PI_DIV_BY_TWO
1667 .................... #endif 1630 .................... #endif
1668 .................... #define PI_DIV_BY_TWO 1.5707963267948966 1631 .................... #define PI_DIV_BY_TWO 1.5707963267948966
1669 .................... #ifdef TWOBYPI 1632 .................... #ifdef TWOBYPI
1670 .................... #undef TWOBYPI 1633 .................... #undef TWOBYPI
1671 .................... #define TWOBYPI 0.6366197723675813 1634 .................... #define TWOBYPI 0.6366197723675813
1672 .................... #endif 1635 .................... #endif
1673 .................... //////////////////////////////////////////////////////////////////////////// 1636 .................... ////////////////////////////////////////////////////////////////////////////
1674 .................... // float cos(float x) 1637 .................... // float cos(float x)
1675 .................... //////////////////////////////////////////////////////////////////////////// 1638 .................... ////////////////////////////////////////////////////////////////////////////
1676 .................... // Description : returns the cosine value of the angle x, which is in radian 1639 .................... // Description : returns the cosine value of the angle x, which is in radian
1677 .................... // Date : 9/20/2001 1640 .................... // Date : 9/20/2001
1678 .................... // 1641 .................... //
1679 .................... float32 cos(float32 x) 1642 .................... float32 cos(float32 x)
1680 .................... { 1643 .................... {
1681 .................... float32 y, t, t2 = 1.0; 1644 .................... float32 y, t, t2 = 1.0;
1682 .................... unsigned int8 quad, i; 1645 .................... unsigned int8 quad, i;
1683 .................... float32 frac; 1646 .................... float32 frac;
1684 .................... float32 p[6] = { //by the series definition for cosine 1647 .................... float32 p[6] = { //by the series definition for cosine
1685 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) 1648 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1686 .................... 0.04166666666667, 1649 .................... 0.04166666666667,
1687 .................... -0.00138888888889, 1650 .................... -0.00138888888889,
1688 .................... 0.00002480158730, 1651 .................... 0.00002480158730,
1689 .................... -0.00000027557319, 1652 .................... -0.00000027557319,
1690 .................... 0.00000000208767, 1653 .................... 0.00000000208767,
1691 .................... //-0.00000000001147, 1654 .................... //-0.00000000001147,
1692 .................... // 0.00000000000005 1655 .................... // 0.00000000000005
1693 .................... }; 1656 .................... };
1694 .................... 1657 ....................
1695 .................... if (x < 0) x = -x; // absolute value of input 1658 .................... if (x < 0) x = -x; // absolute value of input
1696 .................... 1659 ....................
1697 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant 1660 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1698 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 1661 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1699 .................... quad = quad % 4; // quadrant (0 to 3) 1662 .................... quad = quad % 4; // quadrant (0 to 3)
1700 .................... 1663 ....................
1701 .................... if (quad == 0 || quad == 2) 1664 .................... if (quad == 0 || quad == 2)
1702 .................... t = frac * PI_DIV_BY_TWO; 1665 .................... t = frac * PI_DIV_BY_TWO;
1703 .................... else if (quad == 1) 1666 .................... else if (quad == 1)
1704 .................... t = (1-frac) * PI_DIV_BY_TWO; 1667 .................... t = (1-frac) * PI_DIV_BY_TWO;
1705 .................... else // should be 3 1668 .................... else // should be 3
1706 .................... t = (frac-1) * PI_DIV_BY_TWO; 1669 .................... t = (frac-1) * PI_DIV_BY_TWO;
1707 .................... 1670 ....................
1708 .................... y = 1.0; 1671 .................... y = 1.0;
1709 .................... t = t * t; 1672 .................... t = t * t;
1710 .................... for (i = 0; i <= 5; i++) 1673 .................... for (i = 0; i <= 5; i++)
1711 .................... { 1674 .................... {
1712 .................... t2 = t2 * t; 1675 .................... t2 = t2 * t;
1713 .................... y = y + p[i] * t2; 1676 .................... y = y + p[i] * t2;
1714 .................... } 1677 .................... }
1715 .................... 1678 ....................
1716 .................... if (quad == 2 || quad == 1) 1679 .................... if (quad == 2 || quad == 1)
1717 .................... y = -y; // correct sign 1680 .................... y = -y; // correct sign
1718 .................... 1681 ....................
1719 .................... return (y); 1682 .................... return (y);
1720 .................... } 1683 .................... }
1721 .................... 1684 ....................
1722 .................... 1685 ....................
1723 .................... //Overloaded functions for cos() for PCD 1686 .................... //Overloaded functions for cos() for PCD
1724 .................... // Overloaded function cos() for data type - Float48 1687 .................... // Overloaded function cos() for data type - Float48
1725 .................... #if defined(__PCD__) 1688 .................... #if defined(__PCD__)
1726 .................... float48 cos(float48 x) 1689 .................... float48 cos(float48 x)
1727 .................... { 1690 .................... {
1728 .................... float48 y, t, t2 = 1.0; 1691 .................... float48 y, t, t2 = 1.0;
1729 .................... unsigned int8 quad, i; 1692 .................... unsigned int8 quad, i;
1730 .................... float48 frac; 1693 .................... float48 frac;
1731 .................... float48 p[6] = { //by the series definition for cosine 1694 .................... float48 p[6] = { //by the series definition for cosine
1732 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) 1695 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1733 .................... 0.04166666666667, 1696 .................... 0.04166666666667,
1734 .................... -0.00138888888889, 1697 .................... -0.00138888888889,
1735 .................... 0.00002480158730, 1698 .................... 0.00002480158730,
1736 .................... -0.00000027557319, 1699 .................... -0.00000027557319,
1737 .................... 0.00000000208767, 1700 .................... 0.00000000208767,
1738 .................... //-0.00000000001147, 1701 .................... //-0.00000000001147,
1739 .................... // 0.00000000000005 1702 .................... // 0.00000000000005
1740 .................... }; 1703 .................... };
1741 .................... 1704 ....................
1742 .................... if (x < 0) x = -x; // absolute value of input 1705 .................... if (x < 0) x = -x; // absolute value of input
1743 .................... 1706 ....................
1744 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant 1707 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1745 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 1708 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1746 .................... quad = quad % 4; // quadrant (0 to 3) 1709 .................... quad = quad % 4; // quadrant (0 to 3)
1747 .................... 1710 ....................
1748 .................... if (quad == 0 || quad == 2) 1711 .................... if (quad == 0 || quad == 2)
1749 .................... t = frac * PI_DIV_BY_TWO; 1712 .................... t = frac * PI_DIV_BY_TWO;
1750 .................... else if (quad == 1) 1713 .................... else if (quad == 1)
1751 .................... t = (1-frac) * PI_DIV_BY_TWO; 1714 .................... t = (1-frac) * PI_DIV_BY_TWO;
1752 .................... else // should be 3 1715 .................... else // should be 3
1753 .................... t = (frac-1) * PI_DIV_BY_TWO; 1716 .................... t = (frac-1) * PI_DIV_BY_TWO;
1754 .................... 1717 ....................
1755 .................... y = 0.999999999781; 1718 .................... y = 0.999999999781;
1756 .................... t = t * t; 1719 .................... t = t * t;
1757 .................... for (i = 0; i <= 5; i++) 1720 .................... for (i = 0; i <= 5; i++)
1758 .................... { 1721 .................... {
1759 .................... t2 = t2 * t; 1722 .................... t2 = t2 * t;
1760 .................... y = y + p[i] * t2; 1723 .................... y = y + p[i] * t2;
1761 .................... } 1724 .................... }
1762 .................... 1725 ....................
1763 .................... if (quad == 2 || quad == 1) 1726 .................... if (quad == 2 || quad == 1)
1764 .................... y = -y; // correct sign 1727 .................... y = -y; // correct sign
1765 .................... 1728 ....................
1766 .................... return (y); 1729 .................... return (y);
1767 .................... } 1730 .................... }
1768 .................... 1731 ....................
1769 .................... // Overloaded function cos() for data type - Float48 1732 .................... // Overloaded function cos() for data type - Float48
1770 .................... float64 cos(float64 x) 1733 .................... float64 cos(float64 x)
1771 .................... { 1734 .................... {
1772 .................... float64 y, t, t2 = 1.0; 1735 .................... float64 y, t, t2 = 1.0;
1773 .................... unsigned int8 quad, i; 1736 .................... unsigned int8 quad, i;
1774 .................... float64 frac; 1737 .................... float64 frac;
1775 .................... float64 p[6] = { //by the series definition for cosine 1738 .................... float64 p[6] = { //by the series definition for cosine
1776 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) 1739 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1777 .................... 0.04166666666667, 1740 .................... 0.04166666666667,
1778 .................... -0.00138888888889, 1741 .................... -0.00138888888889,
1779 .................... 0.00002480158730, 1742 .................... 0.00002480158730,
1780 .................... -0.00000027557319, 1743 .................... -0.00000027557319,
1781 .................... 0.00000000208767, 1744 .................... 0.00000000208767,
1782 .................... //-0.00000000001147, 1745 .................... //-0.00000000001147,
1783 .................... // 0.00000000000005 1746 .................... // 0.00000000000005
1784 .................... }; 1747 .................... };
1785 .................... 1748 ....................
1786 .................... if (x < 0) x = -x; // absolute value of input 1749 .................... if (x < 0) x = -x; // absolute value of input
1787 .................... 1750 ....................
1788 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant 1751 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1789 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 1752 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1790 .................... quad = quad % 4; // quadrant (0 to 3) 1753 .................... quad = quad % 4; // quadrant (0 to 3)
1791 .................... 1754 ....................
1792 .................... if (quad == 0 || quad == 2) 1755 .................... if (quad == 0 || quad == 2)
1793 .................... t = frac * PI_DIV_BY_TWO; 1756 .................... t = frac * PI_DIV_BY_TWO;
1794 .................... else if (quad == 1) 1757 .................... else if (quad == 1)
1795 .................... t = (1-frac) * PI_DIV_BY_TWO; 1758 .................... t = (1-frac) * PI_DIV_BY_TWO;
1796 .................... else // should be 3 1759 .................... else // should be 3
1797 .................... t = (frac-1) * PI_DIV_BY_TWO; 1760 .................... t = (frac-1) * PI_DIV_BY_TWO;
1798 .................... 1761 ....................
1799 .................... y = 0.999999999781; 1762 .................... y = 0.999999999781;
1800 .................... t = t * t; 1763 .................... t = t * t;
1801 .................... for (i = 0; i <= 5; i++) 1764 .................... for (i = 0; i <= 5; i++)
1802 .................... { 1765 .................... {
1803 .................... t2 = t2 * t; 1766 .................... t2 = t2 * t;
1804 .................... y = y + p[i] * t2; 1767 .................... y = y + p[i] * t2;
1805 .................... } 1768 .................... }
1806 .................... 1769 ....................
1807 .................... if (quad == 2 || quad == 1) 1770 .................... if (quad == 2 || quad == 1)
1808 .................... y = -y; // correct sign 1771 .................... y = -y; // correct sign
1809 .................... 1772 ....................
1810 .................... return (y); 1773 .................... return (y);
1811 .................... } 1774 .................... }
1812 .................... 1775 ....................
1813 .................... #endif 1776 .................... #endif
1814 .................... 1777 ....................
1815 .................... //////////////////////////////////////////////////////////////////////////// 1778 .................... ////////////////////////////////////////////////////////////////////////////
1816 .................... // float sin(float x) 1779 .................... // float sin(float x)
1817 .................... //////////////////////////////////////////////////////////////////////////// 1780 .................... ////////////////////////////////////////////////////////////////////////////
1818 .................... // Description : returns the sine value of the angle x, which is in radian 1781 .................... // Description : returns the sine value of the angle x, which is in radian
1819 .................... // Date : 9/20/2001 1782 .................... // Date : 9/20/2001
1820 .................... // 1783 .................... //
1821 .................... float32 sin(float32 x) 1784 .................... float32 sin(float32 x)
1822 .................... { 1785 .................... {
1823 .................... return cos(x - PI_DIV_BY_TWO); 1786 .................... return cos(x - PI_DIV_BY_TWO);
1824 .................... } 1787 .................... }
1825 .................... 1788 ....................
1826 .................... //Overloaded functions for sin() for PCD 1789 .................... //Overloaded functions for sin() for PCD
1827 .................... // Overloaded function sin() for data type - Float48 1790 .................... // Overloaded function sin() for data type - Float48
1828 .................... #if defined(__PCD__) 1791 .................... #if defined(__PCD__)
1829 .................... float48 sin(float48 x) 1792 .................... float48 sin(float48 x)
1830 .................... { 1793 .................... {
1831 .................... return cos(x - PI_DIV_BY_TWO); 1794 .................... return cos(x - PI_DIV_BY_TWO);
1832 .................... } 1795 .................... }
1833 .................... 1796 ....................
1834 .................... // Overloaded function sin() for data type - Float48 1797 .................... // Overloaded function sin() for data type - Float48
1835 .................... float64 sin(float64 x) 1798 .................... float64 sin(float64 x)
1836 .................... { 1799 .................... {
1837 .................... return cos(x - PI_DIV_BY_TWO); 1800 .................... return cos(x - PI_DIV_BY_TWO);
1838 .................... } 1801 .................... }
1839 .................... #endif 1802 .................... #endif
1840 .................... 1803 ....................
1841 .................... //////////////////////////////////////////////////////////////////////////// 1804 .................... ////////////////////////////////////////////////////////////////////////////
1842 .................... // float tan(float x) 1805 .................... // float tan(float x)
1843 .................... //////////////////////////////////////////////////////////////////////////// 1806 .................... ////////////////////////////////////////////////////////////////////////////
1844 .................... // Description : returns the tangent value of the angle x, which is in radian 1807 .................... // Description : returns the tangent value of the angle x, which is in radian
1845 .................... // Date : 9/20/2001 1808 .................... // Date : 9/20/2001
1846 .................... // 1809 .................... //
1847 .................... float32 tan(float32 x) 1810 .................... float32 tan(float32 x)
1848 .................... { 1811 .................... {
1849 .................... float32 c, s; 1812 .................... float32 c, s;
1850 .................... 1813 ....................
1851 .................... c = cos(x); 1814 .................... c = cos(x);
1852 .................... if (c == 0.0) 1815 .................... if (c == 0.0)
1853 .................... return (1.0e+36); 1816 .................... return (1.0e+36);
1854 .................... 1817 ....................
1855 .................... s = sin(x); 1818 .................... s = sin(x);
1856 .................... return(s/c); 1819 .................... return(s/c);
1857 .................... } 1820 .................... }
1858 .................... //Overloaded functions for tan() for PCD 1821 .................... //Overloaded functions for tan() for PCD
1859 .................... // Overloaded function tan() for data type - Float48 1822 .................... // Overloaded function tan() for data type - Float48
1860 .................... #if defined(__PCD__) 1823 .................... #if defined(__PCD__)
1861 .................... float48 tan(float48 x) 1824 .................... float48 tan(float48 x)
1862 .................... { 1825 .................... {
1863 .................... float48 c, s; 1826 .................... float48 c, s;
1864 .................... 1827 ....................
1865 .................... c = cos(x); 1828 .................... c = cos(x);
1866 .................... if (c == 0.0) 1829 .................... if (c == 0.0)
1867 .................... return (1.0e+36); 1830 .................... return (1.0e+36);
1868 .................... 1831 ....................
1869 .................... s = sin(x); 1832 .................... s = sin(x);
1870 .................... return(s/c); 1833 .................... return(s/c);
1871 .................... } 1834 .................... }
1872 .................... 1835 ....................
1873 .................... // Overloaded function tan() for data type - Float48 1836 .................... // Overloaded function tan() for data type - Float48
1874 .................... float64 tan(float64 x) 1837 .................... float64 tan(float64 x)
1875 .................... { 1838 .................... {
1876 .................... float64 c, s; 1839 .................... float64 c, s;
1877 .................... 1840 ....................
1878 .................... c = cos(x); 1841 .................... c = cos(x);
1879 .................... if (c == 0.0) 1842 .................... if (c == 0.0)
1880 .................... return (1.0e+36); 1843 .................... return (1.0e+36);
1881 .................... 1844 ....................
1882 .................... s = sin(x); 1845 .................... s = sin(x);
1883 .................... return(s/c); 1846 .................... return(s/c);
1884 .................... } 1847 .................... }
1885 .................... #endif 1848 .................... #endif
1886 .................... 1849 ....................
1887 .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290}; 1850 .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290};
1888 .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290}; 1851 .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290};
1889 .................... 1852 ....................
1890 .................... float32 ASIN_COS(float32 x, unsigned int8 n) 1853 .................... float32 ASIN_COS(float32 x, unsigned int8 n)
1891 .................... { 1854 .................... {
1892 .................... float32 y, res, r, y2; 1855 .................... float32 y, res, r, y2;
1893 .................... int1 s; 1856 .................... int1 s;
1894 .................... #ifdef _ERRNO 1857 .................... #ifdef _ERRNO
1895 .................... if(x <-1 || x > 1) 1858 .................... if(x <-1 || x > 1)
1896 .................... { 1859 .................... {
1897 .................... errno=EDOM; 1860 .................... errno=EDOM;
1898 .................... } 1861 .................... }
1899 .................... #endif 1862 .................... #endif
1900 .................... s = 0; 1863 .................... s = 0;
1901 .................... y = x; 1864 .................... y = x;
1902 .................... 1865 ....................
1903 .................... if (x < 0) 1866 .................... if (x < 0)
1904 .................... { 1867 .................... {
1905 .................... s = 1; 1868 .................... s = 1;
1906 .................... y = -y; 1869 .................... y = -y;
1907 .................... } 1870 .................... }
1908 .................... 1871 ....................
1909 .................... if (y > 0.5) 1872 .................... if (y > 0.5)
1910 .................... { 1873 .................... {
1911 .................... y = sqrt((1.0 - y)/2.0); 1874 .................... y = sqrt((1.0 - y)/2.0);
1912 .................... n += 2; 1875 .................... n += 2;
1913 .................... } 1876 .................... }
1914 .................... 1877 ....................
1915 .................... y2=y*y; 1878 .................... y2=y*y;
1916 .................... 1879 ....................
1917 .................... res = pas[0]*y2 + pas[1]; 1880 .................... res = pas[0]*y2 + pas[1];
1918 .................... res = res*y2 + pas[2]; 1881 .................... res = res*y2 + pas[2];
1919 .................... 1882 ....................
1920 .................... r = qas[0]*y2 + qas[1]; 1883 .................... r = qas[0]*y2 + qas[1];
1921 .................... r = r*y2 + qas[2]; 1884 .................... r = r*y2 + qas[2];
1922 .................... 1885 ....................
1923 .................... res = y*res/r; 1886 .................... res = y*res/r;
1924 .................... 1887 ....................
1925 .................... if (n & 2) // |x| > 0.5 1888 .................... if (n & 2) // |x| > 0.5
1926 .................... res = PI_DIV_BY_TWO - 2.0*res; 1889 .................... res = PI_DIV_BY_TWO - 2.0*res;
1927 .................... if (s) 1890 .................... if (s)
1928 .................... res = -res; 1891 .................... res = -res;
1929 .................... if (n & 1) // take arccos 1892 .................... if (n & 1) // take arccos
1930 .................... res = PI_DIV_BY_TWO - res; 1893 .................... res = PI_DIV_BY_TWO - res;
1931 .................... 1894 ....................
1932 .................... return(res); 1895 .................... return(res);
1933 .................... } 1896 .................... }
1934 .................... 1897 ....................
1935 .................... //Overloaded functions for ASIN_COS() for PCD 1898 .................... //Overloaded functions for ASIN_COS() for PCD
1936 .................... // Overloaded function ASIN_COS() for data type - Float48 1899 .................... // Overloaded function ASIN_COS() for data type - Float48
1937 .................... #if defined(__PCD__) 1900 .................... #if defined(__PCD__)
1938 .................... float48 ASIN_COS(float48 x, unsigned int8 n) 1901 .................... float48 ASIN_COS(float48 x, unsigned int8 n)
1939 .................... { 1902 .................... {
1940 .................... float48 y, res, r, y2; 1903 .................... float48 y, res, r, y2;
1941 .................... int1 s; 1904 .................... int1 s;
1942 .................... #ifdef _ERRNO 1905 .................... #ifdef _ERRNO
1943 .................... if(x <-1 || x > 1) 1906 .................... if(x <-1 || x > 1)
1944 .................... { 1907 .................... {
1945 .................... errno=EDOM; 1908 .................... errno=EDOM;
1946 .................... } 1909 .................... }
1947 .................... #endif 1910 .................... #endif
1948 .................... s = 0; 1911 .................... s = 0;
1949 .................... y = x; 1912 .................... y = x;
1950 .................... 1913 ....................
1951 .................... if (x < 0) 1914 .................... if (x < 0)
1952 .................... { 1915 .................... {
1953 .................... s = 1; 1916 .................... s = 1;
1954 .................... y = -y; 1917 .................... y = -y;
1955 .................... } 1918 .................... }
1956 .................... 1919 ....................
1957 .................... if (y > 0.5) 1920 .................... if (y > 0.5)
1958 .................... { 1921 .................... {
1959 .................... y = sqrt((1.0 - y)/2.0); 1922 .................... y = sqrt((1.0 - y)/2.0);
1960 .................... n += 2; 1923 .................... n += 2;
1961 .................... } 1924 .................... }
1962 .................... 1925 ....................
1963 .................... y2=y*y; 1926 .................... y2=y*y;
1964 .................... 1927 ....................
1965 .................... res = pas[0]*y2 + pas[1]; 1928 .................... res = pas[0]*y2 + pas[1];
1966 .................... res = res*y2 + pas[2]; 1929 .................... res = res*y2 + pas[2];
1967 .................... 1930 ....................
1968 .................... r = qas[0]*y2 + qas[1]; 1931 .................... r = qas[0]*y2 + qas[1];
1969 .................... r = r*y2 + qas[2]; 1932 .................... r = r*y2 + qas[2];
1970 .................... 1933 ....................
1971 .................... res = y*res/r; 1934 .................... res = y*res/r;
1972 .................... 1935 ....................
1973 .................... if (n & 2) // |x| > 0.5 1936 .................... if (n & 2) // |x| > 0.5
1974 .................... res = PI_DIV_BY_TWO - 2.0*res; 1937 .................... res = PI_DIV_BY_TWO - 2.0*res;
1975 .................... if (s) 1938 .................... if (s)
1976 .................... res = -res; 1939 .................... res = -res;
1977 .................... if (n & 1) // take arccos 1940 .................... if (n & 1) // take arccos
1978 .................... res = PI_DIV_BY_TWO - res; 1941 .................... res = PI_DIV_BY_TWO - res;
1979 .................... 1942 ....................
1980 .................... return(res); 1943 .................... return(res);
1981 .................... } 1944 .................... }
1982 .................... 1945 ....................
1983 .................... // Overloaded function ASIN_COS() for data type - Float64 1946 .................... // Overloaded function ASIN_COS() for data type - Float64
1984 .................... float64 ASIN_COS(float64 x, unsigned int8 n) 1947 .................... float64 ASIN_COS(float64 x, unsigned int8 n)
1985 .................... { 1948 .................... {
1986 .................... float64 y, res, r, y2; 1949 .................... float64 y, res, r, y2;
1987 .................... int1 s; 1950 .................... int1 s;
1988 .................... #ifdef _ERRNO 1951 .................... #ifdef _ERRNO
1989 .................... if(x <-1 || x > 1) 1952 .................... if(x <-1 || x > 1)
1990 .................... { 1953 .................... {
1991 .................... errno=EDOM; 1954 .................... errno=EDOM;
1992 .................... } 1955 .................... }
1993 .................... #endif 1956 .................... #endif
1994 .................... s = 0; 1957 .................... s = 0;
1995 .................... y = x; 1958 .................... y = x;
1996 .................... 1959 ....................
1997 .................... if (x < 0) 1960 .................... if (x < 0)
1998 .................... { 1961 .................... {
1999 .................... s = 1; 1962 .................... s = 1;
2000 .................... y = -y; 1963 .................... y = -y;
2001 .................... } 1964 .................... }
2002 .................... 1965 ....................
2003 .................... if (y > 0.5) 1966 .................... if (y > 0.5)
2004 .................... { 1967 .................... {
2005 .................... y = sqrt((1.0 - y)/2.0); 1968 .................... y = sqrt((1.0 - y)/2.0);
2006 .................... n += 2; 1969 .................... n += 2;
2007 .................... } 1970 .................... }
2008 .................... 1971 ....................
2009 .................... y2=y*y; 1972 .................... y2=y*y;
2010 .................... 1973 ....................
2011 .................... res = pas[0]*y2 + pas[1]; 1974 .................... res = pas[0]*y2 + pas[1];
2012 .................... res = res*y2 + pas[2]; 1975 .................... res = res*y2 + pas[2];
2013 .................... 1976 ....................
2014 .................... r = qas[0]*y2 + qas[1]; 1977 .................... r = qas[0]*y2 + qas[1];
2015 .................... r = r*y2 + qas[2]; 1978 .................... r = r*y2 + qas[2];
2016 .................... 1979 ....................
2017 .................... res = y*res/r; 1980 .................... res = y*res/r;
2018 .................... 1981 ....................
2019 .................... if (n & 2) // |x| > 0.5 1982 .................... if (n & 2) // |x| > 0.5
2020 .................... res = PI_DIV_BY_TWO - 2.0*res; 1983 .................... res = PI_DIV_BY_TWO - 2.0*res;
2021 .................... if (s) 1984 .................... if (s)
2022 .................... res = -res; 1985 .................... res = -res;
2023 .................... if (n & 1) // take arccos 1986 .................... if (n & 1) // take arccos
2024 .................... res = PI_DIV_BY_TWO - res; 1987 .................... res = PI_DIV_BY_TWO - res;
2025 .................... 1988 ....................
2026 .................... return(res); 1989 .................... return(res);
2027 .................... } 1990 .................... }
2028 .................... #endif 1991 .................... #endif
2029 .................... 1992 ....................
2030 .................... //////////////////////////////////////////////////////////////////////////// 1993 .................... ////////////////////////////////////////////////////////////////////////////
2031 .................... // float asin(float x) 1994 .................... // float asin(float x)
2032 .................... //////////////////////////////////////////////////////////////////////////// 1995 .................... ////////////////////////////////////////////////////////////////////////////
2033 .................... // Description : returns the arcsine value of the value x. 1996 .................... // Description : returns the arcsine value of the value x.
2034 .................... // Date : N/A 1997 .................... // Date : N/A
2035 .................... // 1998 .................... //
2036 .................... float32 asin(float32 x) 1999 .................... float32 asin(float32 x)
2037 .................... { 2000 .................... {
2038 .................... float32 r; 2001 .................... float32 r;
2039 .................... 2002 ....................
2040 .................... r = ASIN_COS(x, 0); 2003 .................... r = ASIN_COS(x, 0);
2041 .................... return(r); 2004 .................... return(r);
2042 .................... } 2005 .................... }
2043 .................... //Overloaded functions for asin() for PCD 2006 .................... //Overloaded functions for asin() for PCD
2044 .................... // Overloaded function asin() for data type - Float48 2007 .................... // Overloaded function asin() for data type - Float48
2045 .................... #if defined(__PCD__) 2008 .................... #if defined(__PCD__)
2046 .................... float48 asin(float48 x) 2009 .................... float48 asin(float48 x)
2047 .................... { 2010 .................... {
2048 .................... float48 r; 2011 .................... float48 r;
2049 .................... 2012 ....................
2050 .................... r = ASIN_COS(x, 0); 2013 .................... r = ASIN_COS(x, 0);
2051 .................... return(r); 2014 .................... return(r);
2052 .................... } 2015 .................... }
2053 .................... 2016 ....................
2054 .................... // Overloaded function asin() for data type - Float64 2017 .................... // Overloaded function asin() for data type - Float64
2055 .................... float64 asin(float64 x) 2018 .................... float64 asin(float64 x)
2056 .................... { 2019 .................... {
2057 .................... float64 r; 2020 .................... float64 r;
2058 .................... 2021 ....................
2059 .................... r = ASIN_COS(x, 0); 2022 .................... r = ASIN_COS(x, 0);
2060 .................... return(r); 2023 .................... return(r);
2061 .................... } 2024 .................... }
2062 .................... #endif 2025 .................... #endif
2063 .................... 2026 ....................
2064 .................... //////////////////////////////////////////////////////////////////////////// 2027 .................... ////////////////////////////////////////////////////////////////////////////
2065 .................... // float acos(float x) 2028 .................... // float acos(float x)
2066 .................... //////////////////////////////////////////////////////////////////////////// 2029 .................... ////////////////////////////////////////////////////////////////////////////
2067 .................... // Description : returns the arccosine value of the value x. 2030 .................... // Description : returns the arccosine value of the value x.
2068 .................... // Date : N/A 2031 .................... // Date : N/A
2069 .................... // 2032 .................... //
2070 .................... float32 acos(float32 x) 2033 .................... float32 acos(float32 x)
2071 .................... { 2034 .................... {
2072 .................... float32 r; 2035 .................... float32 r;
2073 .................... 2036 ....................
2074 .................... r = ASIN_COS(x, 1); 2037 .................... r = ASIN_COS(x, 1);
2075 .................... return(r); 2038 .................... return(r);
2076 .................... } 2039 .................... }
2077 .................... //Overloaded functions for acos() for PCD 2040 .................... //Overloaded functions for acos() for PCD
2078 .................... // Overloaded function acos() for data type - Float48 2041 .................... // Overloaded function acos() for data type - Float48
2079 .................... #if defined(__PCD__) 2042 .................... #if defined(__PCD__)
2080 .................... float48 acos(float48 x) 2043 .................... float48 acos(float48 x)
2081 .................... { 2044 .................... {
2082 .................... float48 r; 2045 .................... float48 r;
2083 .................... 2046 ....................
2084 .................... r = ASIN_COS(x, 1); 2047 .................... r = ASIN_COS(x, 1);
2085 .................... return(r); 2048 .................... return(r);
2086 .................... } 2049 .................... }
2087 .................... 2050 ....................
2088 .................... // Overloaded function acos() for data type - Float64 2051 .................... // Overloaded function acos() for data type - Float64
2089 .................... float64 acos(float64 x) 2052 .................... float64 acos(float64 x)
2090 .................... { 2053 .................... {
2091 .................... float64 r; 2054 .................... float64 r;
2092 .................... 2055 ....................
2093 .................... r = ASIN_COS(x, 1); 2056 .................... r = ASIN_COS(x, 1);
2094 .................... return(r); 2057 .................... return(r);
2095 .................... } 2058 .................... }
2096 .................... #endif 2059 .................... #endif
2097 .................... 2060 ....................
2098 .................... float32 const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; 2061 .................... float32 const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457};
2099 .................... float32 const qat[4] = {1.0000000, 11.368190, 28.982246, 19.818457}; 2062 .................... float32 const qat[4] = {1.0000000, 11.368190, 28.982246, 19.818457};
2100 .................... 2063 ....................
2101 .................... //////////////////////////////////////////////////////////////////////////// 2064 .................... ////////////////////////////////////////////////////////////////////////////
2102 .................... // float atan(float x) 2065 .................... // float atan(float x)
2103 .................... //////////////////////////////////////////////////////////////////////////// 2066 .................... ////////////////////////////////////////////////////////////////////////////
2104 .................... // Description : returns the arctangent value of the value x. 2067 .................... // Description : returns the arctangent value of the value x.
2105 .................... // Date : N/A 2068 .................... // Date : N/A
2106 .................... // 2069 .................... //
2107 .................... float32 atan(float32 x) 2070 .................... float32 atan(float32 x)
2108 .................... { 2071 .................... {
2109 .................... float32 y, res, r; 2072 .................... float32 y, res, r;
2110 .................... int1 s, flag; 2073 .................... int1 s, flag;
2111 .................... 2074 ....................
2112 .................... s = 0; 2075 .................... s = 0;
2113 .................... flag = 0; 2076 .................... flag = 0;
2114 .................... y = x; 2077 .................... y = x;
2115 .................... 2078 ....................
2116 .................... if (x < 0) 2079 .................... if (x < 0)
2117 .................... { 2080 .................... {
2118 .................... s = 1; 2081 .................... s = 1;
2119 .................... y = -y; 2082 .................... y = -y;
2120 .................... } 2083 .................... }
2121 .................... 2084 ....................
2122 .................... if (y > 1.0) 2085 .................... if (y > 1.0)
2123 .................... { 2086 .................... {
2124 .................... y = 1.0/y; 2087 .................... y = 1.0/y;
2125 .................... flag = 1; 2088 .................... flag = 1;
2126 .................... } 2089 .................... }
2127 .................... 2090 ....................
2128 .................... res = pat[0]*y*y + pat[1]; 2091 .................... res = pat[0]*y*y + pat[1];
2129 .................... res = res*y*y + pat[2]; 2092 .................... res = res*y*y + pat[2];
2130 .................... res = res*y*y + pat[3]; 2093 .................... res = res*y*y + pat[3];
2131 .................... 2094 ....................
2132 .................... r = qat[0]*y*y + qat[1]; 2095 .................... r = qat[0]*y*y + qat[1];
2133 .................... r = r*y*y + qat[2]; 2096 .................... r = r*y*y + qat[2];
2134 .................... r = r*y*y + qat[3]; 2097 .................... r = r*y*y + qat[3];
2135 .................... 2098 ....................
2136 .................... res = y*res/r; 2099 .................... res = y*res/r;
2137 .................... 2100 ....................
2138 .................... 2101 ....................
2139 .................... if (flag) // for |x| > 1 2102 .................... if (flag) // for |x| > 1
2140 .................... res = PI_DIV_BY_TWO - res; 2103 .................... res = PI_DIV_BY_TWO - res;
2141 .................... if (s) 2104 .................... if (s)
2142 .................... res = -res; 2105 .................... res = -res;
2143 .................... 2106 ....................
2144 .................... return(res); 2107 .................... return(res);
2145 .................... } 2108 .................... }
2146 .................... //Overloaded functions for atan() for PCD 2109 .................... //Overloaded functions for atan() for PCD
2147 .................... // Overloaded function atan() for data type - Float48 2110 .................... // Overloaded function atan() for data type - Float48
2148 .................... #if defined(__PCD__) 2111 .................... #if defined(__PCD__)
2149 .................... float48 atan(float48 x) 2112 .................... float48 atan(float48 x)
2150 .................... { 2113 .................... {
2151 .................... float48 y, res, r; 2114 .................... float48 y, res, r;
2152 .................... int1 s, flag; 2115 .................... int1 s, flag;
2153 .................... 2116 ....................
2154 .................... s = 0; 2117 .................... s = 0;
2155 .................... flag = 0; 2118 .................... flag = 0;
2156 .................... y = x; 2119 .................... y = x;
2157 .................... 2120 ....................
2158 .................... if (x < 0) 2121 .................... if (x < 0)
2159 .................... { 2122 .................... {
2160 .................... s = 1; 2123 .................... s = 1;
2161 .................... y = -y; 2124 .................... y = -y;
2162 .................... } 2125 .................... }
2163 .................... 2126 ....................
2164 .................... if (y > 1.0) 2127 .................... if (y > 1.0)
2165 .................... { 2128 .................... {
2166 .................... y = 1.0/y; 2129 .................... y = 1.0/y;
2167 .................... flag = 1; 2130 .................... flag = 1;
2168 .................... } 2131 .................... }
2169 .................... 2132 ....................
2170 .................... res = pat[0]*y*y + pat[1]; 2133 .................... res = pat[0]*y*y + pat[1];
2171 .................... res = res*y*y + pat[2]; 2134 .................... res = res*y*y + pat[2];
2172 .................... res = res*y*y + pat[3]; 2135 .................... res = res*y*y + pat[3];
2173 .................... 2136 ....................
2174 .................... r = qat[0]*y*y + qat[1]; 2137 .................... r = qat[0]*y*y + qat[1];
2175 .................... r = r*y*y + qat[2]; 2138 .................... r = r*y*y + qat[2];
2176 .................... r = r*y*y + qat[3]; 2139 .................... r = r*y*y + qat[3];
2177 .................... 2140 ....................
2178 .................... res = y*res/r; 2141 .................... res = y*res/r;
2179 .................... 2142 ....................
2180 .................... 2143 ....................
2181 .................... if (flag) // for |x| > 1 2144 .................... if (flag) // for |x| > 1
2182 .................... res = PI_DIV_BY_TWO - res; 2145 .................... res = PI_DIV_BY_TWO - res;
2183 .................... if (s) 2146 .................... if (s)
2184 .................... res = -res; 2147 .................... res = -res;
2185 .................... 2148 ....................
2186 .................... return(res); 2149 .................... return(res);
2187 .................... } 2150 .................... }
2188 .................... 2151 ....................
2189 .................... // Overloaded function atan() for data type - Float64 2152 .................... // Overloaded function atan() for data type - Float64
2190 .................... float64 atan(float64 x) 2153 .................... float64 atan(float64 x)
2191 .................... { 2154 .................... {
2192 .................... float64 y, res, r; 2155 .................... float64 y, res, r;
2193 .................... int1 s, flag; 2156 .................... int1 s, flag;
2194 .................... 2157 ....................
2195 .................... s = 0; 2158 .................... s = 0;
2196 .................... flag = 0; 2159 .................... flag = 0;
2197 .................... y = x; 2160 .................... y = x;
2198 .................... 2161 ....................
2199 .................... if (x < 0) 2162 .................... if (x < 0)
2200 .................... { 2163 .................... {
2201 .................... s = 1; 2164 .................... s = 1;
2202 .................... y = -y; 2165 .................... y = -y;
2203 .................... } 2166 .................... }
2204 .................... 2167 ....................
2205 .................... if (y > 1.0) 2168 .................... if (y > 1.0)
2206 .................... { 2169 .................... {
2207 .................... y = 1.0/y; 2170 .................... y = 1.0/y;
2208 .................... flag = 1; 2171 .................... flag = 1;
2209 .................... } 2172 .................... }
2210 .................... 2173 ....................
2211 .................... res = pat[0]*y*y + pat[1]; 2174 .................... res = pat[0]*y*y + pat[1];
2212 .................... res = res*y*y + pat[2]; 2175 .................... res = res*y*y + pat[2];
2213 .................... res = res*y*y + pat[3]; 2176 .................... res = res*y*y + pat[3];
2214 .................... 2177 ....................
2215 .................... r = qat[0]*y*y + qat[1]; 2178 .................... r = qat[0]*y*y + qat[1];
2216 .................... r = r*y*y + qat[2]; 2179 .................... r = r*y*y + qat[2];
2217 .................... r = r*y*y + qat[3]; 2180 .................... r = r*y*y + qat[3];
2218 .................... 2181 ....................
2219 .................... res = y*res/r; 2182 .................... res = y*res/r;
2220 .................... 2183 ....................
2221 .................... 2184 ....................
2222 .................... if (flag) // for |x| > 1 2185 .................... if (flag) // for |x| > 1
2223 .................... res = PI_DIV_BY_TWO - res; 2186 .................... res = PI_DIV_BY_TWO - res;
2224 .................... if (s) 2187 .................... if (s)
2225 .................... res = -res; 2188 .................... res = -res;
2226 .................... 2189 ....................
2227 .................... return(res); 2190 .................... return(res);
2228 .................... } 2191 .................... }
2229 .................... #endif 2192 .................... #endif
2230 .................... 2193 ....................
2231 .................... ///////////////////////////////////////////////////////////////////////////// 2194 .................... /////////////////////////////////////////////////////////////////////////////
2232 .................... // float atan2(float y, float x) 2195 .................... // float atan2(float y, float x)
2233 .................... ///////////////////////////////////////////////////////////////////////////// 2196 .................... /////////////////////////////////////////////////////////////////////////////
2234 .................... // Description :computes the principal value of arc tangent of y/x, using the 2197 .................... // Description :computes the principal value of arc tangent of y/x, using the
2235 .................... // signs of both the arguments to determine the quadrant of the return value 2198 .................... // signs of both the arguments to determine the quadrant of the return value
2236 .................... // Returns : returns the arc tangent of y/x. 2199 .................... // Returns : returns the arc tangent of y/x.
2237 .................... // Date : N/A 2200 .................... // Date : N/A
2238 .................... // 2201 .................... //
2239 .................... 2202 ....................
2240 .................... float32 atan2(float32 y,float32 x) 2203 .................... float32 atan2(float32 y,float32 x)
2241 .................... { 2204 .................... {
2242 .................... float32 z; 2205 .................... float32 z;
2243 .................... int1 sign; 2206 .................... int1 sign;
2244 .................... unsigned int8 quad; 2207 .................... unsigned int8 quad;
2245 .................... sign=0; 2208 .................... sign=0;
2246 .................... quad=0; //quadrant 2209 .................... quad=0; //quadrant
2247 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 2210 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2248 .................... if(y<0.0) 2211 .................... if(y<0.0)
2249 .................... { 2212 .................... {
2250 .................... sign=1; 2213 .................... sign=1;
2251 .................... y=-y; 2214 .................... y=-y;
2252 .................... } 2215 .................... }
2253 .................... if(x<0.0) 2216 .................... if(x<0.0)
2254 .................... { 2217 .................... {
2255 .................... x=-x; 2218 .................... x=-x;
2256 .................... } 2219 .................... }
2257 .................... if (x==0.0) 2220 .................... if (x==0.0)
2258 .................... { 2221 .................... {
2259 .................... if(y==0.0) 2222 .................... if(y==0.0)
2260 .................... { 2223 .................... {
2261 .................... #ifdef _ERRNO 2224 .................... #ifdef _ERRNO
2262 .................... { 2225 .................... {
2263 .................... errno=EDOM; 2226 .................... errno=EDOM;
2264 .................... } 2227 .................... }
2265 .................... #endif 2228 .................... #endif
2266 .................... } 2229 .................... }
2267 .................... else 2230 .................... else
2268 .................... { 2231 .................... {
2269 .................... if(sign) 2232 .................... if(sign)
2270 .................... { 2233 .................... {
2271 .................... return (-(PI_DIV_BY_TWO)); 2234 .................... return (-(PI_DIV_BY_TWO));
2272 .................... } 2235 .................... }
2273 .................... else 2236 .................... else
2274 .................... { 2237 .................... {
2275 .................... return (PI_DIV_BY_TWO); 2238 .................... return (PI_DIV_BY_TWO);
2276 .................... } 2239 .................... }
2277 .................... } 2240 .................... }
2278 .................... } 2241 .................... }
2279 .................... else 2242 .................... else
2280 .................... { 2243 .................... {
2281 .................... z=y/x; 2244 .................... z=y/x;
2282 .................... switch(quad) 2245 .................... switch(quad)
2283 .................... { 2246 .................... {
2284 .................... case 1: 2247 .................... case 1:
2285 .................... { 2248 .................... {
2286 .................... return atan(z); 2249 .................... return atan(z);
2287 .................... break; 2250 .................... break;
2288 .................... } 2251 .................... }
2289 .................... case 2: 2252 .................... case 2:
2290 .................... { 2253 .................... {
2291 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 2254 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2292 .................... return (PI-atan(z)); 2255 .................... return (PI-atan(z));
2293 .................... break; 2256 .................... break;
2294 .................... } 2257 .................... }
2295 .................... case 3: 2258 .................... case 3:
2296 .................... { 2259 .................... {
2297 .................... return (atan(z)-PI); 2260 .................... return (atan(z)-PI);
2298 .................... break; 2261 .................... break;
2299 .................... } 2262 .................... }
2300 .................... case 4: 2263 .................... case 4:
2301 .................... { 2264 .................... {
2302 .................... return (-atan(z)); 2265 .................... return (-atan(z));
2303 .................... break; 2266 .................... break;
2304 .................... } 2267 .................... }
2305 .................... } 2268 .................... }
2306 .................... } 2269 .................... }
2307 .................... } 2270 .................... }
2308 .................... 2271 ....................
2309 .................... //Overloaded functions for atan2() for PCD 2272 .................... //Overloaded functions for atan2() for PCD
2310 .................... // Overloaded function atan2() for data type - Float48 2273 .................... // Overloaded function atan2() for data type - Float48
2311 .................... #if defined(__PCD__) 2274 .................... #if defined(__PCD__)
2312 .................... float48 atan2(float48 y,float48 x) 2275 .................... float48 atan2(float48 y,float48 x)
2313 .................... { 2276 .................... {
2314 .................... float48 z; 2277 .................... float48 z;
2315 .................... int1 sign; 2278 .................... int1 sign;
2316 .................... unsigned int8 quad; 2279 .................... unsigned int8 quad;
2317 .................... sign=0; 2280 .................... sign=0;
2318 .................... quad=0; //quadrant 2281 .................... quad=0; //quadrant
2319 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 2282 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2320 .................... if(y<0.0) 2283 .................... if(y<0.0)
2321 .................... { 2284 .................... {
2322 .................... sign=1; 2285 .................... sign=1;
2323 .................... y=-y; 2286 .................... y=-y;
2324 .................... } 2287 .................... }
2325 .................... if(x<0.0) 2288 .................... if(x<0.0)
2326 .................... { 2289 .................... {
2327 .................... x=-x; 2290 .................... x=-x;
2328 .................... } 2291 .................... }
2329 .................... if (x==0.0) 2292 .................... if (x==0.0)
2330 .................... { 2293 .................... {
2331 .................... if(y==0.0) 2294 .................... if(y==0.0)
2332 .................... { 2295 .................... {
2333 .................... #ifdef _ERRNO 2296 .................... #ifdef _ERRNO
2334 .................... { 2297 .................... {
2335 .................... errno=EDOM; 2298 .................... errno=EDOM;
2336 .................... } 2299 .................... }
2337 .................... #endif 2300 .................... #endif
2338 .................... } 2301 .................... }
2339 .................... else 2302 .................... else
2340 .................... { 2303 .................... {
2341 .................... if(sign) 2304 .................... if(sign)
2342 .................... { 2305 .................... {
2343 .................... return (-(PI_DIV_BY_TWO)); 2306 .................... return (-(PI_DIV_BY_TWO));
2344 .................... } 2307 .................... }
2345 .................... else 2308 .................... else
2346 .................... { 2309 .................... {
2347 .................... return (PI_DIV_BY_TWO); 2310 .................... return (PI_DIV_BY_TWO);
2348 .................... } 2311 .................... }
2349 .................... } 2312 .................... }
2350 .................... } 2313 .................... }
2351 .................... else 2314 .................... else
2352 .................... { 2315 .................... {
2353 .................... z=y/x; 2316 .................... z=y/x;
2354 .................... switch(quad) 2317 .................... switch(quad)
2355 .................... { 2318 .................... {
2356 .................... case 1: 2319 .................... case 1:
2357 .................... { 2320 .................... {
2358 .................... return atan(z); 2321 .................... return atan(z);
2359 .................... break; 2322 .................... break;
2360 .................... } 2323 .................... }
2361 .................... case 2: 2324 .................... case 2:
2362 .................... { 2325 .................... {
2363 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 2326 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2364 .................... return (PI-atan(z)); 2327 .................... return (PI-atan(z));
2365 .................... break; 2328 .................... break;
2366 .................... } 2329 .................... }
2367 .................... case 3: 2330 .................... case 3:
2368 .................... { 2331 .................... {
2369 .................... return (atan(z)-PI); 2332 .................... return (atan(z)-PI);
2370 .................... break; 2333 .................... break;
2371 .................... } 2334 .................... }
2372 .................... case 4: 2335 .................... case 4:
2373 .................... { 2336 .................... {
2374 .................... return (-atan(z)); 2337 .................... return (-atan(z));
2375 .................... break; 2338 .................... break;
2376 .................... } 2339 .................... }
2377 .................... } 2340 .................... }
2378 .................... } 2341 .................... }
2379 .................... } 2342 .................... }
2380 .................... 2343 ....................
2381 .................... // Overloaded function atan2() for data type - Float64 2344 .................... // Overloaded function atan2() for data type - Float64
2382 .................... float64 atan2(float64 y,float64 x) 2345 .................... float64 atan2(float64 y,float64 x)
2383 .................... { 2346 .................... {
2384 .................... float64 z; 2347 .................... float64 z;
2385 .................... int1 sign; 2348 .................... int1 sign;
2386 .................... unsigned int8 quad; 2349 .................... unsigned int8 quad;
2387 .................... sign=0; 2350 .................... sign=0;
2388 .................... quad=0; //quadrant 2351 .................... quad=0; //quadrant
2389 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 2352 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2390 .................... if(y<0.0) 2353 .................... if(y<0.0)
2391 .................... { 2354 .................... {
2392 .................... sign=1; 2355 .................... sign=1;
2393 .................... y=-y; 2356 .................... y=-y;
2394 .................... } 2357 .................... }
2395 .................... if(x<0.0) 2358 .................... if(x<0.0)
2396 .................... { 2359 .................... {
2397 .................... x=-x; 2360 .................... x=-x;
2398 .................... } 2361 .................... }
2399 .................... if (x==0.0) 2362 .................... if (x==0.0)
2400 .................... { 2363 .................... {
2401 .................... if(y==0.0) 2364 .................... if(y==0.0)
2402 .................... { 2365 .................... {
2403 .................... #ifdef _ERRNO 2366 .................... #ifdef _ERRNO
2404 .................... { 2367 .................... {
2405 .................... errno=EDOM; 2368 .................... errno=EDOM;
2406 .................... } 2369 .................... }
2407 .................... #endif 2370 .................... #endif
2408 .................... } 2371 .................... }
2409 .................... else 2372 .................... else
2410 .................... { 2373 .................... {
2411 .................... if(sign) 2374 .................... if(sign)
2412 .................... { 2375 .................... {
2413 .................... return (-(PI_DIV_BY_TWO)); 2376 .................... return (-(PI_DIV_BY_TWO));
2414 .................... } 2377 .................... }
2415 .................... else 2378 .................... else
2416 .................... { 2379 .................... {
2417 .................... return (PI_DIV_BY_TWO); 2380 .................... return (PI_DIV_BY_TWO);
2418 .................... } 2381 .................... }
2419 .................... } 2382 .................... }
2420 .................... } 2383 .................... }
2421 .................... else 2384 .................... else
2422 .................... { 2385 .................... {
2423 .................... z=y/x; 2386 .................... z=y/x;
2424 .................... switch(quad) 2387 .................... switch(quad)
2425 .................... { 2388 .................... {
2426 .................... case 1: 2389 .................... case 1:
2427 .................... { 2390 .................... {
2428 .................... return atan(z); 2391 .................... return atan(z);
2429 .................... break; 2392 .................... break;
2430 .................... } 2393 .................... }
2431 .................... case 2: 2394 .................... case 2:
2432 .................... { 2395 .................... {
2433 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 2396 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2434 .................... return (PI-atan(z)); 2397 .................... return (PI-atan(z));
2435 .................... break; 2398 .................... break;
2436 .................... } 2399 .................... }
2437 .................... case 3: 2400 .................... case 3:
2438 .................... { 2401 .................... {
2439 .................... return (atan(z)-PI); 2402 .................... return (atan(z)-PI);
2440 .................... break; 2403 .................... break;
2441 .................... } 2404 .................... }
2442 .................... case 4: 2405 .................... case 4:
2443 .................... { 2406 .................... {
2444 .................... return (-atan(z)); 2407 .................... return (-atan(z));
2445 .................... break; 2408 .................... break;
2446 .................... } 2409 .................... }
2447 .................... } 2410 .................... }
2448 .................... } 2411 .................... }
2449 .................... } 2412 .................... }
2450 .................... #endif 2413 .................... #endif
2451 .................... 2414 ....................
2452 .................... //////////////////// Hyperbolic functions //////////////////// 2415 .................... //////////////////// Hyperbolic functions ////////////////////
2453 .................... 2416 ....................
2454 .................... //////////////////////////////////////////////////////////////////////////// 2417 .................... ////////////////////////////////////////////////////////////////////////////
2455 .................... // float cosh(float x) 2418 .................... // float cosh(float x)
2456 .................... //////////////////////////////////////////////////////////////////////////// 2419 .................... ////////////////////////////////////////////////////////////////////////////
2457 .................... // Description : Computes the hyperbolic cosine value of x 2420 .................... // Description : Computes the hyperbolic cosine value of x
2458 .................... // Returns : returns the hyperbolic cosine value of x 2421 .................... // Returns : returns the hyperbolic cosine value of x
2459 .................... // Date : N/A 2422 .................... // Date : N/A
2460 .................... // 2423 .................... //
2461 .................... 2424 ....................
2462 .................... float32 cosh(float32 x) 2425 .................... float32 cosh(float32 x)
2463 .................... { 2426 .................... {
2464 .................... return ((exp(x)+exp(-x))/2); 2427 .................... return ((exp(x)+exp(-x))/2);
2465 .................... } 2428 .................... }
2466 .................... //Overloaded functions for cosh() for PCD 2429 .................... //Overloaded functions for cosh() for PCD
2467 .................... // Overloaded function cosh() for data type - Float48 2430 .................... // Overloaded function cosh() for data type - Float48
2468 .................... #if defined(__PCD__) 2431 .................... #if defined(__PCD__)
2469 .................... float48 cosh(float48 x) 2432 .................... float48 cosh(float48 x)
2470 .................... { 2433 .................... {
2471 .................... return ((exp(x)+exp(-x))/2); 2434 .................... return ((exp(x)+exp(-x))/2);
2472 .................... } 2435 .................... }
2473 .................... 2436 ....................
2474 .................... // Overloaded function cosh() for data type - Float64 2437 .................... // Overloaded function cosh() for data type - Float64
2475 .................... float64 cosh(float64 x) 2438 .................... float64 cosh(float64 x)
2476 .................... { 2439 .................... {
2477 .................... return ((exp(x)+exp(-x))/2); 2440 .................... return ((exp(x)+exp(-x))/2);
2478 .................... } 2441 .................... }
2479 .................... #endif 2442 .................... #endif
2480 .................... 2443 ....................
2481 .................... //////////////////////////////////////////////////////////////////////////// 2444 .................... ////////////////////////////////////////////////////////////////////////////
2482 .................... // float sinh(float x) 2445 .................... // float sinh(float x)
2483 .................... //////////////////////////////////////////////////////////////////////////// 2446 .................... ////////////////////////////////////////////////////////////////////////////
2484 .................... // Description : Computes the hyperbolic sine value of x 2447 .................... // Description : Computes the hyperbolic sine value of x
2485 .................... // Returns : returns the hyperbolic sine value of x 2448 .................... // Returns : returns the hyperbolic sine value of x
2486 .................... // Date : N/A 2449 .................... // Date : N/A
2487 .................... // 2450 .................... //
2488 .................... 2451 ....................
2489 .................... float32 sinh(float32 x) 2452 .................... float32 sinh(float32 x)
2490 .................... { 2453 .................... {
2491 .................... 2454 ....................
2492 .................... return ((exp(x) - exp(-x))/2); 2455 .................... return ((exp(x) - exp(-x))/2);
2493 .................... } 2456 .................... }
2494 .................... //Overloaded functions for sinh() for PCD 2457 .................... //Overloaded functions for sinh() for PCD
2495 .................... // Overloaded function sinh() for data type - Float48 2458 .................... // Overloaded function sinh() for data type - Float48
2496 .................... #if defined(__PCD__) 2459 .................... #if defined(__PCD__)
2497 .................... float48 sinh(float48 x) 2460 .................... float48 sinh(float48 x)
2498 .................... { 2461 .................... {
2499 .................... 2462 ....................
2500 .................... return ((exp(x) - exp(-x))/2); 2463 .................... return ((exp(x) - exp(-x))/2);
2501 .................... } 2464 .................... }
2502 .................... 2465 ....................
2503 .................... // Overloaded function sinh() for data type - Float48 2466 .................... // Overloaded function sinh() for data type - Float48
2504 .................... float64 sinh(float64 x) 2467 .................... float64 sinh(float64 x)
2505 .................... { 2468 .................... {
2506 .................... 2469 ....................
2507 .................... return ((exp(x) - exp(-x))/2); 2470 .................... return ((exp(x) - exp(-x))/2);
2508 .................... } 2471 .................... }
2509 .................... #endif 2472 .................... #endif
2510 .................... 2473 ....................
2511 .................... //////////////////////////////////////////////////////////////////////////// 2474 .................... ////////////////////////////////////////////////////////////////////////////
2512 .................... // float tanh(float x) 2475 .................... // float tanh(float x)
2513 .................... //////////////////////////////////////////////////////////////////////////// 2476 .................... ////////////////////////////////////////////////////////////////////////////
2514 .................... // Description : Computes the hyperbolic tangent value of x 2477 .................... // Description : Computes the hyperbolic tangent value of x
2515 .................... // Returns : returns the hyperbolic tangent value of x 2478 .................... // Returns : returns the hyperbolic tangent value of x
2516 .................... // Date : N/A 2479 .................... // Date : N/A
2517 .................... // 2480 .................... //
2518 .................... 2481 ....................
2519 .................... float32 tanh(float32 x) 2482 .................... float32 tanh(float32 x)
2520 .................... { 2483 .................... {
2521 .................... return(sinh(x)/cosh(x)); 2484 .................... return(sinh(x)/cosh(x));
2522 .................... } 2485 .................... }
2523 .................... //Overloaded functions for tanh() for PCD 2486 .................... //Overloaded functions for tanh() for PCD
2524 .................... // Overloaded function tanh() for data type - Float48 2487 .................... // Overloaded function tanh() for data type - Float48
2525 .................... #if defined(__PCD__) 2488 .................... #if defined(__PCD__)
2526 .................... float48 tanh(float48 x) 2489 .................... float48 tanh(float48 x)
2527 .................... { 2490 .................... {
2528 .................... return(sinh(x)/cosh(x)); 2491 .................... return(sinh(x)/cosh(x));
2529 .................... } 2492 .................... }
2530 .................... 2493 ....................
2531 .................... // Overloaded function tanh() for data type - Float64 2494 .................... // Overloaded function tanh() for data type - Float64
2532 .................... float64 tanh(float64 x) 2495 .................... float64 tanh(float64 x)
2533 .................... { 2496 .................... {
2534 .................... return(sinh(x)/cosh(x)); 2497 .................... return(sinh(x)/cosh(x));
2535 .................... } 2498 .................... }
2536 .................... #endif 2499 .................... #endif
2537 .................... 2500 ....................
2538 .................... //////////////////////////////////////////////////////////////////////////// 2501 .................... ////////////////////////////////////////////////////////////////////////////
2539 .................... // float frexp(float x, signed int *exp) 2502 .................... // float frexp(float x, signed int *exp)
2540 .................... //////////////////////////////////////////////////////////////////////////// 2503 .................... ////////////////////////////////////////////////////////////////////////////
2541 .................... // Description : breaks a floating point number into a normalized fraction and an integral 2504 .................... // Description : breaks a floating point number into a normalized fraction and an integral
2542 .................... // power of 2. It stores the integer in the signed int object pointed to by exp. 2505 .................... // power of 2. It stores the integer in the signed int object pointed to by exp.
2543 .................... // Returns : returns the value x, such that x is a double with magnitude in the interval 2506 .................... // Returns : returns the value x, such that x is a double with magnitude in the interval
2544 .................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero, 2507 .................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero,
2545 .................... // both parts of the result are zero. 2508 .................... // both parts of the result are zero.
2546 .................... // Date : N/A 2509 .................... // Date : N/A
2547 .................... // 2510 .................... //
2548 .................... 2511 ....................
2549 .................... #define LOG2 .30102999566398119521 2512 .................... #define LOG2 .30102999566398119521
2550 .................... float32 frexp(float32 x, signed int8 *exp) 2513 .................... float32 frexp(float32 x, signed int8 *exp)
2551 .................... { 2514 .................... {
2552 .................... float32 res; 2515 .................... float32 res;
2553 .................... int1 sign = 0; 2516 .................... int1 sign = 0;
2554 .................... if(x == 0.0) 2517 .................... if(x == 0.0)
2555 .................... { 2518 .................... {
2556 .................... *exp=0; 2519 .................... *exp=0;
2557 .................... return (0.0); 2520 .................... return (0.0);
2558 .................... } 2521 .................... }
2559 .................... if(x < 0.0) 2522 .................... if(x < 0.0)
2560 .................... { 2523 .................... {
2561 .................... x=-x; 2524 .................... x=-x;
2562 .................... sign=1; 2525 .................... sign=1;
2563 .................... } 2526 .................... }
2564 .................... if (x > 1.0) 2527 .................... if (x > 1.0)
2565 .................... { 2528 .................... {
2566 .................... *exp=(ceil(log10(x)/LOG2)); 2529 .................... *exp=(ceil(log10(x)/LOG2));
2567 .................... res=x/(pow(2, *exp)); 2530 .................... res=x/(pow(2, *exp));
2568 .................... if (res == 1) 2531 .................... if (res == 1)
2569 .................... { 2532 .................... {
2570 .................... *exp=*exp+1; 2533 .................... *exp=*exp+1;
2571 .................... res=.5; 2534 .................... res=.5;
2572 .................... } 2535 .................... }
2573 .................... } 2536 .................... }
2574 .................... else 2537 .................... else
2575 .................... { 2538 .................... {
2576 .................... if(x < 0.5) 2539 .................... if(x < 0.5)
2577 .................... { 2540 .................... {
2578 .................... *exp=-1; 2541 .................... *exp=-1;
2579 .................... res=x*2; 2542 .................... res=x*2;
2580 .................... } 2543 .................... }
2581 .................... else 2544 .................... else
2582 .................... { 2545 .................... {
2583 .................... *exp=0; 2546 .................... *exp=0;
2584 .................... res=x; 2547 .................... res=x;
2585 .................... } 2548 .................... }
2586 .................... } 2549 .................... }
2587 .................... if(sign) 2550 .................... if(sign)
2588 .................... { 2551 .................... {
2589 .................... res=-res; 2552 .................... res=-res;
2590 .................... } 2553 .................... }
2591 .................... return res; 2554 .................... return res;
2592 .................... } 2555 .................... }
2593 .................... 2556 ....................
2594 .................... //Overloaded functions for frexp() for PCD 2557 .................... //Overloaded functions for frexp() for PCD
2595 .................... // Overloaded function frexp() for data type - Float48 2558 .................... // Overloaded function frexp() for data type - Float48
2596 .................... #if defined(__PCD__) 2559 .................... #if defined(__PCD__)
2597 .................... float48 frexp(float48 x, signed int8 *exp) 2560 .................... float48 frexp(float48 x, signed int8 *exp)
2598 .................... { 2561 .................... {
2599 .................... float48 res; 2562 .................... float48 res;
2600 .................... int1 sign = 0; 2563 .................... int1 sign = 0;
2601 .................... if(x == 0.0) 2564 .................... if(x == 0.0)
2602 .................... { 2565 .................... {
2603 .................... *exp=0; 2566 .................... *exp=0;
2604 .................... return (0.0); 2567 .................... return (0.0);
2605 .................... } 2568 .................... }
2606 .................... if(x < 0.0) 2569 .................... if(x < 0.0)
2607 .................... { 2570 .................... {
2608 .................... x=-x; 2571 .................... x=-x;
2609 .................... sign=1; 2572 .................... sign=1;
2610 .................... } 2573 .................... }
2611 .................... if (x > 1.0) 2574 .................... if (x > 1.0)
2612 .................... { 2575 .................... {
2613 .................... *exp=(ceil(log10(x)/LOG2)); 2576 .................... *exp=(ceil(log10(x)/LOG2));
2614 .................... res=x/(pow(2, *exp)); 2577 .................... res=x/(pow(2, *exp));
2615 .................... if (res == 1) 2578 .................... if (res == 1)
2616 .................... { 2579 .................... {
2617 .................... *exp=*exp+1; 2580 .................... *exp=*exp+1;
2618 .................... res=.5; 2581 .................... res=.5;
2619 .................... } 2582 .................... }
2620 .................... } 2583 .................... }
2621 .................... else 2584 .................... else
2622 .................... { 2585 .................... {
2623 .................... if(x < 0.5) 2586 .................... if(x < 0.5)
2624 .................... { 2587 .................... {
2625 .................... *exp=-1; 2588 .................... *exp=-1;
2626 .................... res=x*2; 2589 .................... res=x*2;
2627 .................... } 2590 .................... }
2628 .................... else 2591 .................... else
2629 .................... { 2592 .................... {
2630 .................... *exp=0; 2593 .................... *exp=0;
2631 .................... res=x; 2594 .................... res=x;
2632 .................... } 2595 .................... }
2633 .................... } 2596 .................... }
2634 .................... if(sign) 2597 .................... if(sign)
2635 .................... { 2598 .................... {
2636 .................... res=-res; 2599 .................... res=-res;
2637 .................... } 2600 .................... }
2638 .................... return res; 2601 .................... return res;
2639 .................... } 2602 .................... }
2640 .................... 2603 ....................
2641 .................... // Overloaded function frexp() for data type - Float64 2604 .................... // Overloaded function frexp() for data type - Float64
2642 .................... float64 frexp(float64 x, signed int8 *exp) 2605 .................... float64 frexp(float64 x, signed int8 *exp)
2643 .................... { 2606 .................... {
2644 .................... float64 res; 2607 .................... float64 res;
2645 .................... int1 sign = 0; 2608 .................... int1 sign = 0;
2646 .................... if(x == 0.0) 2609 .................... if(x == 0.0)
2647 .................... { 2610 .................... {
2648 .................... *exp=0; 2611 .................... *exp=0;
2649 .................... return (0.0); 2612 .................... return (0.0);
2650 .................... } 2613 .................... }
2651 .................... if(x < 0.0) 2614 .................... if(x < 0.0)
2652 .................... { 2615 .................... {
2653 .................... x=-x; 2616 .................... x=-x;
2654 .................... sign=1; 2617 .................... sign=1;
2655 .................... } 2618 .................... }
2656 .................... if (x > 1.0) 2619 .................... if (x > 1.0)
2657 .................... { 2620 .................... {
2658 .................... *exp=(ceil(log10(x)/LOG2)); 2621 .................... *exp=(ceil(log10(x)/LOG2));
2659 .................... res=x/(pow(2, *exp)); 2622 .................... res=x/(pow(2, *exp));
2660 .................... if (res == 1) 2623 .................... if (res == 1)
2661 .................... { 2624 .................... {
2662 .................... *exp=*exp+1; 2625 .................... *exp=*exp+1;
2663 .................... res=.5; 2626 .................... res=.5;
2664 .................... } 2627 .................... }
2665 .................... } 2628 .................... }
2666 .................... else 2629 .................... else
2667 .................... { 2630 .................... {
2668 .................... if(x < 0.5) 2631 .................... if(x < 0.5)
2669 .................... { 2632 .................... {
2670 .................... *exp=-1; 2633 .................... *exp=-1;
2671 .................... res=x*2; 2634 .................... res=x*2;
2672 .................... } 2635 .................... }
2673 .................... else 2636 .................... else
2674 .................... { 2637 .................... {
2675 .................... *exp=0; 2638 .................... *exp=0;
2676 .................... res=x; 2639 .................... res=x;
2677 .................... } 2640 .................... }
2678 .................... } 2641 .................... }
2679 .................... if(sign) 2642 .................... if(sign)
2680 .................... { 2643 .................... {
2681 .................... res=-res; 2644 .................... res=-res;
2682 .................... } 2645 .................... }
2683 .................... return res; 2646 .................... return res;
2684 .................... } 2647 .................... }
2685 .................... #endif 2648 .................... #endif
2686 .................... 2649 ....................
2687 .................... ////////////////////////////////////////////////////////////////////////////// 2650 .................... //////////////////////////////////////////////////////////////////////////////
2688 .................... // float ldexp(float x, signed int *exp) 2651 .................... // float ldexp(float x, signed int *exp)
2689 .................... ////////////////////////////////////////////////////////////////////////////// 2652 .................... //////////////////////////////////////////////////////////////////////////////
2690 .................... // Description : multiplies a floating point number by an integral power of 2. 2653 .................... // Description : multiplies a floating point number by an integral power of 2.
2691 .................... // Returns : returns the value of x times 2 raised to the power exp. 2654 .................... // Returns : returns the value of x times 2 raised to the power exp.
2692 .................... // Date : N/A 2655 .................... // Date : N/A
2693 .................... // 2656 .................... //
2694 .................... 2657 ....................
2695 .................... float32 ldexp(float32 value, signed int8 exp) 2658 .................... float32 ldexp(float32 value, signed int8 exp)
2696 .................... { 2659 .................... {
2697 .................... return (value * pow(2,exp)); 2660 .................... return (value * pow(2,exp));
2698 .................... } 2661 .................... }
2699 .................... //Overloaded functions for ldexp() for PCD 2662 .................... //Overloaded functions for ldexp() for PCD
2700 .................... // Overloaded function ldexp() for data type - Float48 2663 .................... // Overloaded function ldexp() for data type - Float48
2701 .................... 2664 ....................
2702 .................... #if defined(__PCD__) 2665 .................... #if defined(__PCD__)
2703 .................... float48 ldexp(float48 value, signed int8 exp) 2666 .................... float48 ldexp(float48 value, signed int8 exp)
2704 .................... { 2667 .................... {
2705 .................... return (value * pow(2,exp)); 2668 .................... return (value * pow(2,exp));
2706 .................... } 2669 .................... }
2707 .................... // Overloaded function ldexp() for data type - Float64 2670 .................... // Overloaded function ldexp() for data type - Float64
2708 .................... float64 ldexp(float64 value, signed int8 exp) 2671 .................... float64 ldexp(float64 value, signed int8 exp)
2709 .................... { 2672 .................... {
2710 .................... return (value * pow(2,exp)); 2673 .................... return (value * pow(2,exp));
2711 .................... } 2674 .................... }
2712 .................... #endif 2675 .................... #endif
2713 .................... 2676 ....................
2714 .................... #endif 2677 .................... #endif
2715 .................... 2678 ....................
2716 .................... 2679 ....................
2717 .................... void main() 2680 .................... void main()
2718 .................... { 2681 .................... {
2719 * 2682 *
2720 0266: CLRF 04 2683 023C: CLRF 04
2721 0267: BCF 03.7 2684 023D: BCF 03.7
2722 0268: MOVLW 1F 2685 023E: MOVLW 1F
2723 0269: ANDWF 03,F 2686 023F: ANDWF 03,F
2724 026A: MOVLW 71 2687 0240: MOVLW 71
2725 026B: BSF 03.5 2688 0241: BSF 03.5
2726 026C: MOVWF 0F 2689 0242: MOVWF 0F
2727 026D: MOVF 0F,W 2690 0243: MOVF 0F,W
2728 026E: BCF 03.5 2691 0244: BSF 03.6
2729 026F: BCF 20.7 2692 0245: BCF 07.3
2730 0270: MOVF 20,W 2693 0246: MOVLW 0C
2731 0271: BSF 03.5 2694 0247: BCF 03.6
-   2695 0248: MOVWF 19
-   2696 0249: MOVLW A2
2732 0272: MOVWF 07 2697 024A: MOVWF 18
-   2698 024B: MOVLW 90
2733 0273: BCF 03.5 2699 024C: BCF 03.5
2734 0274: BSF 07.7 2700 024D: MOVWF 18
2735 0275: BSF 03.5 2701 024E: BSF 03.5
2736 0276: BSF 03.6 2702 024F: BSF 03.6
2737 0277: MOVF 09,W 2703 0250: MOVF 09,W
2738 0278: ANDLW C0 2704 0251: ANDLW C0
2739 0279: MOVWF 09 2705 0252: MOVWF 09
2740 027A: BCF 03.6 2706 0253: BCF 03.6
2741 027B: BCF 1F.4 2707 0254: BCF 1F.4
2742 027C: BCF 1F.5 2708 0255: BCF 1F.5
2743 027D: MOVLW 00 2709 0256: MOVLW 00
2744 027E: BSF 03.6 2710 0257: BSF 03.6
2745 027F: MOVWF 08 2711 0258: MOVWF 08
2746 0280: BCF 03.5 2712 0259: BCF 03.5
2747 0281: CLRF 07 2713 025A: CLRF 07
2748 0282: CLRF 08 2714 025B: CLRF 08
2749 0283: CLRF 09 2715 025C: CLRF 09
2750 * -  
2751 028D: CLRF 34 -  
2752 028E: CLRF 33 -  
2753 .................... float last,b,anemo_speed; -  
2754 .................... unsigned int16 anemo_round=0; -  
2755 .................... unsigned int16 i; -  
2756 .................... -  
2757 .................... //signed int16 X,Y,Z; -  
2758 .................... setup_adc_ports(NO_ANALOGS|VSS_VDD); 2716 .................... setup_adc_ports(NO_ANALOGS|VSS_VDD);
-   2717 *
2759 028F: BSF 03.5 2718 0266: BSF 03.5
2760 0290: BSF 03.6 2719 0267: BSF 03.6
2761 0291: MOVF 09,W 2720 0268: MOVF 09,W
2762 0292: ANDLW C0 2721 0269: ANDLW C0
2763 0293: MOVWF 09 2722 026A: MOVWF 09
2764 0294: BCF 03.6 2723 026B: BCF 03.6
2765 0295: BCF 1F.4 2724 026C: BCF 1F.4
2766 0296: BCF 1F.5 2725 026D: BCF 1F.5
2767 0297: MOVLW 00 2726 026E: MOVLW 00
2768 0298: BSF 03.6 2727 026F: BSF 03.6
2769 0299: MOVWF 08 2728 0270: MOVWF 08
2770 .................... setup_adc(ADC_CLOCK_DIV_2); 2729 .................... setup_adc(ADC_CLOCK_DIV_2);
2771 029A: BCF 03.5 2730 0271: BCF 03.5
2772 029B: BCF 03.6 2731 0272: BCF 03.6
2773 029C: BCF 1F.6 2732 0273: BCF 1F.6
2774 029D: BCF 1F.7 2733 0274: BCF 1F.7
2775 029E: BSF 03.5 2734 0275: BSF 03.5
2776 029F: BCF 1F.7 2735 0276: BCF 1F.7
2777 02A0: BCF 03.5 2736 0277: BCF 03.5
2778 02A1: BSF 1F.0 2737 0278: BSF 1F.0
2779 .................... setup_spi(SPI_SS_DISABLED); 2738 .................... setup_spi(SPI_SS_DISABLED);
2780 02A2: BCF 14.5 2739 0279: BCF 14.5
2781 02A3: BCF 20.5 2740 027A: BCF 20.5
2782 02A4: MOVF 20,W 2741 027B: MOVF 20,W
2783 02A5: BSF 03.5 2742 027C: BSF 03.5
2784 02A6: MOVWF 07 2743 027D: MOVWF 07
2785 02A7: BCF 03.5 2744 027E: BCF 03.5
2786 02A8: BSF 20.4 2745 027F: BSF 20.4
2787 02A9: MOVF 20,W 2746 0280: MOVF 20,W
2788 02AA: BSF 03.5 2747 0281: BSF 03.5
2789 02AB: MOVWF 07 2748 0282: MOVWF 07
2790 02AC: BCF 03.5 2749 0283: BCF 03.5
2791 02AD: BCF 20.3 2750 0284: BCF 20.3
2792 02AE: MOVF 20,W 2751 0285: MOVF 20,W
2793 02AF: BSF 03.5 2752 0286: BSF 03.5
2794 02B0: MOVWF 07 2753 0287: MOVWF 07
2795 02B1: MOVLW 01 2754 0288: MOVLW 01
2796 02B2: BCF 03.5 2755 0289: BCF 03.5
2797 02B3: MOVWF 14 2756 028A: MOVWF 14
2798 02B4: MOVLW 00 2757 028B: MOVLW 00
2799 02B5: BSF 03.5 2758 028C: BSF 03.5
2800 02B6: MOVWF 14 2759 028D: MOVWF 14
2801 .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 2760 .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
2802 02B7: MOVF 01,W 2761 028E: MOVF 01,W
2803 02B8: ANDLW C7 2762 028F: ANDLW C7
2804 02B9: IORLW 08 2763 0290: IORLW 08
2805 02BA: MOVWF 01 2764 0291: MOVWF 01
2806 .................... setup_timer_1(T1_DISABLED); 2765 .................... setup_timer_1(T1_DISABLED);
2807 02BB: BCF 03.5 2766 0292: BCF 03.5
2808 02BC: CLRF 10 2767 0293: CLRF 10
2809 .................... setup_timer_2(T2_DISABLED,0,1); 2768 .................... setup_timer_2(T2_DISABLED,0,1);
2810 02BD: MOVLW 00 2769 0294: MOVLW 00
2811 02BE: MOVWF 78 2770 0295: MOVWF 78
2812 02BF: MOVWF 12 2771 0296: MOVWF 12
2813 02C0: MOVLW 00 2772 0297: MOVLW 00
2814 02C1: BSF 03.5 2773 0298: BSF 03.5
2815 02C2: MOVWF 12 2774 0299: MOVWF 12
2816 .................... setup_ccp1(CCP_OFF); 2775 .................... setup_ccp1(CCP_OFF);
2817 02C3: BCF 03.5 2776 029A: BCF 03.5
2818 02C4: BSF 20.2 2777 029B: BSF 20.2
2819 02C5: MOVF 20,W 2778 029C: MOVF 20,W
2820 02C6: BSF 03.5 2779 029D: BSF 03.5
2821 02C7: MOVWF 07 2780 029E: MOVWF 07
2822 02C8: BCF 03.5 2781 029F: BCF 03.5
2823 02C9: CLRF 17 2782 02A0: CLRF 17
2824 02CA: BSF 03.5 2783 02A1: BSF 03.5
2825 02CB: CLRF 1B 2784 02A2: CLRF 1B
2826 02CC: CLRF 1C 2785 02A3: CLRF 1C
2827 02CD: MOVLW 01 2786 02A4: MOVLW 01
2828 02CE: MOVWF 1D 2787 02A5: MOVWF 1D
2829 .................... setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard 2788 .................... setup_comparator(NC_NC_NC_NC);
2830 02CF: BCF 03.5 2789 02A6: BCF 03.5
2831 02D0: BSF 03.6 2790 02A7: BSF 03.6
2832 02D1: CLRF 07 2791 02A8: CLRF 07
2833 02D2: CLRF 08 2792 02A9: CLRF 08
2834 02D3: CLRF 09 2793 02AA: CLRF 09
2835 .................... 2794 ....................
2836 .................... printf("Magnetometr: \r\n",); 2795 .................... printf("Magnetometr: \r\n",);
2837 02D4: MOVLW 04 2796 02AB: MOVLW 04
2838 02D5: MOVWF 0D 2797 02AC: MOVWF 0D
2839 02D6: MOVLW 00 2798 02AD: MOVLW 00
2840 02D7: MOVWF 0F 2799 02AE: MOVWF 0F
2841 02D8: BCF 03.6 2800 02AF: BCF 03.6
2842 02D9: CALL 043 2801 02B0: CALL 021
2843 .................... printf("(c)mlab JACHO 2013: \r\n",); 2802 .................... printf("(c)mlab.cz kaklik 2013: \r\n",);
2844 02DA: MOVLW 0D 2803 02B1: MOVLW 0D
2845 02DB: BSF 03.6 2804 02B2: BSF 03.6
2846 02DC: MOVWF 0D 2805 02B3: MOVWF 0D
2847 02DD: MOVLW 00 2806 02B4: MOVLW 00
2848 02DE: MOVWF 0F 2807 02B5: MOVWF 0F
2849 02DF: BCF 03.6 2808 02B6: BCF 03.6
2850 02E0: CALL 043 2809 02B7: CALL 021
2851 .................... printf("X, Y, Z \r\n",); 2810 .................... printf("X, Y, Z \r\n",);
2852 02E1: MOVLW 19 2811 02B8: MOVLW 1B
2853 02E2: BSF 03.6 2812 02B9: BSF 03.6
2854 02E3: MOVWF 0D 2813 02BA: MOVWF 0D
2855 02E4: MOVLW 00 2814 02BB: MOVLW 00
2856 02E5: MOVWF 0F 2815 02BC: MOVWF 0F
2857 02E6: BCF 03.6 2816 02BD: BCF 03.6
2858 02E7: CALL 043 2817 02BE: CALL 021
2859 .................... 2818 ....................
2860 .................... 2819 ....................
2861 .................... // Init the HMC5883L. Set Mode register for 2820 .................... // Init the HMC5883L. Set Mode register for
2862 .................... // continuous measurements. 2821 .................... // continuous measurements.
2863 .................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18); // no average, maximal update range 2822 .................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18); // no average, maximal update range
2864 02E8: CLRF 37 2823 02BF: CLRF 27
2865 02E9: MOVLW 18 2824 02C0: MOVLW 18
2866 02EA: MOVWF 38 2825 02C1: MOVWF 28
2867 02EB: CALL 0D3 2826 02C2: CALL 0B3
2868 .................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0xE0); // maximal range 2827 .................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0x00); // minimal range
2869 02EC: MOVLW 01 2828 02C3: MOVLW 01
2870 02ED: MOVWF 37 2829 02C4: MOVWF 27
2871 02EE: MOVLW E0 -  
2872 02EF: MOVWF 38 2830 02C5: CLRF 28
2873 02F0: CALL 0D3 2831 02C6: CALL 0B3
2874 .................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00); 2832 .................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00);
2875 02F1: MOVLW 02 2833 02C7: MOVLW 02
2876 02F2: MOVWF 37 2834 02C8: MOVWF 27
2877 02F3: CLRF 38 2835 02C9: CLRF 28
2878 02F4: CALL 0D3 2836 02CA: CALL 0B3
2879 .................... 2837 ....................
2880 .................... // Continuously read and display the x,y,z results. 2838 .................... // Continuously read and display the x,y,z results.
2881 .................... // Wait at least 67 ms between reads, re the HMC5883L data sheet. 2839 .................... // Wait at least 67 ms between reads, re the HMC5883L data sheet.
2882 .................... 2840 ....................
2883 .................... 2841 ....................
2884 .................... while(TRUE) 2842 .................... while(TRUE)
2885 .................... { 2843 .................... {
2886 .................... 2844 ....................
2887 .................... hmc5883l_read_data(); 2845 .................... hmc5883l_read_data();
2888 02F5: GOTO 151 2846 02CB: GOTO 131
2889 .................... printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z); 2847 .................... printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z);
2890 02F6: MOVLW 00 2848 02CC: MOVLW 00
2891 02F7: MOVWF 04 2849 02CD: MOVWF 04
2892 02F8: MOVF 22,W 2850 02CE: MOVF 22,W
2893 02F9: MOVWF 38 2851 02CF: MOVWF 28
2894 02FA: MOVF 21,W 2852 02D0: MOVF 21,W
2895 02FB: MOVWF 37 2853 02D1: MOVWF 27
2896 02FC: CALL 1D0 2854 02D2: CALL 1B0
2897 02FD: MOVLW 20 2855 02D3: MOVLW 20
-   2856 02D4: BTFSS 0C.4
2898 02FE: MOVWF 40 2857 02D5: GOTO 2D4
2899 02FF: CALL 01F 2858 02D6: MOVWF 19
2900 0300: MOVLW 00 2859 02D7: MOVLW 00
2901 0301: MOVWF 04 2860 02D8: MOVWF 04
2902 0302: MOVF 24,W 2861 02D9: MOVF 24,W
2903 0303: MOVWF 38 2862 02DA: MOVWF 28
2904 0304: MOVF 23,W 2863 02DB: MOVF 23,W
2905 0305: MOVWF 37 2864 02DC: MOVWF 27
2906 0306: CALL 1D0 2865 02DD: CALL 1B0
2907 0307: MOVLW 20 2866 02DE: MOVLW 20
-   2867 02DF: BTFSS 0C.4
2908 0308: MOVWF 40 2868 02E0: GOTO 2DF
2909 0309: CALL 01F 2869 02E1: MOVWF 19
2910 030A: MOVLW 00 2870 02E2: MOVLW 00
2911 030B: MOVWF 04 2871 02E3: MOVWF 04
2912 030C: MOVF 26,W 2872 02E4: MOVF 26,W
2913 030D: MOVWF 38 2873 02E5: MOVWF 28
2914 030E: MOVF 25,W 2874 02E6: MOVF 25,W
2915 030F: MOVWF 37 2875 02E7: MOVWF 27
2916 0310: CALL 1D0 2876 02E8: CALL 1B0
2917 0311: MOVLW 20 2877 02E9: MOVLW 20
-   2878 02EA: BTFSS 0C.4
2918 0312: MOVWF 40 2879 02EB: GOTO 2EA
2919 0313: CALL 01F 2880 02EC: MOVWF 19
2920 0314: MOVLW 0A 2881 02ED: MOVLW 0A
-   2882 02EE: BTFSS 0C.4
2921 0315: MOVWF 40 2883 02EF: GOTO 2EE
2922 0316: CALL 01F 2884 02F0: MOVWF 19
2923 0317: MOVLW 0D 2885 02F1: MOVLW 0D
-   2886 02F2: BTFSS 0C.4
2924 0318: MOVWF 40 2887 02F3: GOTO 2F2
2925 0319: CALL 01F 2888 02F4: MOVWF 19
2926 .................... delay_ms(100); 2889 .................... delay_ms(100);
2927 031A: MOVLW 64 2890 02F5: MOVLW 64
2928 031B: MOVWF 37 2891 02F6: MOVWF 27
2929 031C: GOTO 250 2892 02F7: GOTO 226
2930 .................... } 2893 .................... }
2931 031D: GOTO 2F5 2894 02F8: GOTO 2CB
2932 .................... 2895 ....................
2933 .................... } 2896 .................... }
2934 .................... 2897 ....................
2935 .................... 2898 ....................
2936 .................... 2899 ....................
2937 031E: SLEEP 2900 02F9: SLEEP
2938   2901  
2939 Configuration Fuses: 2902 Configuration Fuses:
2940 Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG 2903 Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG
2941 Word 2: 3FFF NOWRT BORV40 2904 Word 2: 3FFF NOWRT BORV40