Rev Author Line No. Line
2998 kaklik 1 CCS PCM C Compiler, Version 4.106, 47914 17-V-13 12:14
2926 jacho 2  
2984 kaklik 3 Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst
2926 jacho 4  
2998 kaklik 5 ROM used: 799 words (10%)
2926 jacho 6 Largest free fragment is 2048
2988 kaklik 7 RAM used: 28 (8%) at main() level
8 38 (10%) worst case
2985 kaklik 9 Stack: 2 locations
2926 jacho 10  
11 *
2985 kaklik 12 0000: MOVLW 02
2926 jacho 13 0001: MOVWF 0A
2998 kaklik 14 0002: GOTO 266
2926 jacho 15 0003: NOP
16 .................... #include "main.h"
17 .................... #include <16F887.h>
18 .................... //////// Standard Header file for the PIC16F887 device ////////////////
19 .................... #device PIC16F887
20 .................... #list
21 ....................
22 .................... #device adc=8
23 ....................
2984 kaklik 24 .................... #FUSES NOWDT //No Watch Dog Timer
25 .................... #FUSES INTRC //Internal RC Osc
26 .................... #FUSES NOPUT //No Power Up Timer
27 .................... #FUSES MCLR //Master Clear pin enabled
28 .................... #FUSES NOPROTECT //Code not protected from reading
29 .................... #FUSES NOCPD //No EE protection
30 .................... #FUSES NOBROWNOUT //No brownout reset
31 .................... #FUSES IESO //Internal External Switch Over mode 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
34 .................... #FUSES NODEBUG //No Debug mode for ICD
35 .................... #FUSES NOWRT //Program memory not write protected
36 .................... #FUSES BORV40 //Brownout reset at 4.0V
2926 jacho 37 ....................
38 .................... #use delay(clock=8000000)
39 *
2998 kaklik 40 0250: MOVLW 37
41 0251: MOVWF 04
42 0252: BCF 03.7
43 0253: MOVF 00,W
44 0254: BTFSC 03.2
45 0255: GOTO 263
46 0256: MOVLW 02
47 0257: MOVWF 78
48 0258: CLRF 77
49 0259: DECFSZ 77,F
50 025A: GOTO 259
51 025B: DECFSZ 78,F
52 025C: GOTO 258
53 025D: MOVLW 97
54 025E: MOVWF 77
55 025F: DECFSZ 77,F
56 0260: GOTO 25F
57 0261: DECFSZ 00,F
58 0262: GOTO 256
59 0263: BCF 0A.3
60 0264: BCF 0A.4
61 0265: GOTO 31D (RETURN)
62 .................... #use i2c(Master,Slow,sda=PIN_C4,scl=PIN_C3)
2926 jacho 63 *
2998 kaklik 64 0089: MOVLW 08
65 008A: MOVWF 78
66 008B: NOP
67 008C: BCF 07.3
68 008D: BCF 20.3
69 008E: MOVF 20,W
70 008F: BSF 03.5
71 0090: MOVWF 07
72 0091: NOP
73 0092: BCF 03.5
74 0093: RLF 3D,F
75 0094: BCF 07.4
76 0095: BTFSS 03.0
77 0096: GOTO 09D
78 0097: BSF 20.4
79 0098: MOVF 20,W
80 0099: BSF 03.5
81 009A: MOVWF 07
82 009B: GOTO 0A1
83 009C: BCF 03.5
84 009D: BCF 20.4
85 009E: MOVF 20,W
86 009F: BSF 03.5
87 00A0: MOVWF 07
88 00A1: NOP
89 00A2: BCF 03.5
90 00A3: BSF 20.3
91 00A4: MOVF 20,W
92 00A5: BSF 03.5
93 00A6: MOVWF 07
94 00A7: BCF 03.5
95 00A8: BTFSS 07.3
96 00A9: GOTO 0A8
97 00AA: DECFSZ 78,F
98 00AB: GOTO 08B
99 00AC: NOP
100 00AD: BCF 07.3
101 00AE: BCF 20.3
102 00AF: MOVF 20,W
103 00B0: BSF 03.5
104 00B1: MOVWF 07
105 00B2: NOP
106 00B3: BCF 03.5
107 00B4: BSF 20.4
108 00B5: MOVF 20,W
109 00B6: BSF 03.5
110 00B7: MOVWF 07
111 00B8: NOP
112 00B9: NOP
113 00BA: BCF 03.5
114 00BB: BSF 20.3
115 00BC: MOVF 20,W
116 00BD: BSF 03.5
117 00BE: MOVWF 07
118 00BF: BCF 03.5
119 00C0: BTFSS 07.3
120 00C1: GOTO 0C0
121 00C2: CLRF 78
122 00C3: NOP
123 00C4: BTFSC 07.4
124 00C5: BSF 78.0
125 00C6: BCF 07.3
126 00C7: BCF 20.3
127 00C8: MOVF 20,W
128 00C9: BSF 03.5
129 00CA: MOVWF 07
130 00CB: BCF 03.5
131 00CC: BCF 07.4
132 00CD: BCF 20.4
133 00CE: MOVF 20,W
134 00CF: BSF 03.5
135 00D0: MOVWF 07
136 00D1: BCF 03.5
137 00D2: RETURN
2984 kaklik 138 *
2998 kaklik 139 010C: MOVLW 08
140 010D: MOVWF 3E
141 010E: MOVF 77,W
142 010F: MOVWF 3F
143 0110: BSF 20.4
144 0111: MOVF 20,W
145 0112: BSF 03.5
146 0113: MOVWF 07
147 0114: NOP
148 0115: BCF 03.5
149 0116: BSF 20.3
150 0117: MOVF 20,W
151 0118: BSF 03.5
152 0119: MOVWF 07
153 011A: BCF 03.5
154 011B: BTFSS 07.3
155 011C: GOTO 11B
156 011D: BTFSC 07.4
157 011E: BSF 03.0
158 011F: BTFSS 07.4
159 0120: BCF 03.0
160 0121: RLF 78,F
161 0122: NOP
162 0123: BCF 20.3
163 0124: MOVF 20,W
164 0125: BSF 03.5
165 0126: MOVWF 07
166 0127: BCF 03.5
167 0128: BCF 07.3
168 0129: DECFSZ 3E,F
169 012A: GOTO 110
170 012B: BSF 20.4
171 012C: MOVF 20,W
172 012D: BSF 03.5
173 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
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
208 .................... #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,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
2926 jacho 246 ....................
2984 kaklik 247 ....................
2926 jacho 248 .................... #include "HMC5883L.h"
2998 kaklik 249 .................... // i2c slave addresses
250 .................... #define HMC5883L_WRT_ADDR 0x3C
251 .................... #define HMC5883L_READ_ADDR 0x3D
2926 jacho 252 ....................
2998 kaklik 253 .................... // Register addresses
254 .................... #define HMC5883L_CFG_A_REG 0x00
255 .................... #define HMC5883L_CFG_B_REG 0x01
256 .................... #define HMC5883L_MODE_REG 0x02
257 .................... #define HMC5883L_X_MSB_REG 0x03
2926 jacho 258 ....................
259 .................... //Konstanty nastavujici rozsah
260 .................... //pro void set_mag_roz (unsigned int8 h)
261 .................... #define MAG_ROZ088 0x00
262 .................... #define MAG_ROZ130 0x20
263 .................... #define MAG_ROZ190 0x40
264 .................... #define MAG_ROZ250 0x60
265 .................... #define MAG_ROZ400 0x80
266 .................... #define MAG_ROZ470 0xA0
267 .................... #define MAG_ROZ560 0xC0
268 .................... #define MAG_ROZ810 0xE0
269 ....................
270 ....................
271 .................... #include "HMC5883L.c"
2985 kaklik 272 .................... //------------------------------
273 .................... // Low level routines
274 .................... //------------------------------
275 .................... void hmc5883l_write_reg(int8 reg, int8 data)
2926 jacho 276 .................... {
2985 kaklik 277 .................... i2c_start();
2984 kaklik 278 *
2998 kaklik 279 00D3: BSF 20.4
280 00D4: MOVF 20,W
281 00D5: BSF 03.5
282 00D6: MOVWF 07
283 00D7: NOP
284 00D8: BCF 03.5
285 00D9: BSF 20.3
286 00DA: MOVF 20,W
287 00DB: BSF 03.5
288 00DC: MOVWF 07
289 00DD: NOP
290 00DE: BCF 03.5
291 00DF: BCF 07.4
292 00E0: BCF 20.4
293 00E1: MOVF 20,W
294 00E2: BSF 03.5
295 00E3: MOVWF 07
296 00E4: NOP
297 00E5: BCF 03.5
298 00E6: BCF 07.3
299 00E7: BCF 20.3
300 00E8: MOVF 20,W
301 00E9: BSF 03.5
302 00EA: MOVWF 07
2985 kaklik 303 .................... i2c_write(HMC5883L_WRT_ADDR);
2998 kaklik 304 00EB: MOVLW 3C
305 00EC: BCF 03.5
306 00ED: MOVWF 3D
307 00EE: CALL 089
2985 kaklik 308 .................... i2c_write(reg);
2998 kaklik 309 00EF: MOVF 37,W
310 00F0: MOVWF 3D
311 00F1: CALL 089
2985 kaklik 312 .................... i2c_write(data);
2998 kaklik 313 00F2: MOVF 38,W
314 00F3: MOVWF 3D
315 00F4: CALL 089
2985 kaklik 316 .................... i2c_stop();
2998 kaklik 317 00F5: BCF 20.4
318 00F6: MOVF 20,W
319 00F7: BSF 03.5
320 00F8: MOVWF 07
321 00F9: NOP
322 00FA: BCF 03.5
323 00FB: BSF 20.3
324 00FC: MOVF 20,W
325 00FD: BSF 03.5
326 00FE: MOVWF 07
327 00FF: BCF 03.5
328 0100: BTFSS 07.3
329 0101: GOTO 100
330 0102: NOP
331 0103: GOTO 104
332 0104: NOP
333 0105: BSF 20.4
334 0106: MOVF 20,W
335 0107: BSF 03.5
336 0108: MOVWF 07
337 0109: NOP
2926 jacho 338 .................... }
2998 kaklik 339 010A: BCF 03.5
340 010B: RETURN
2926 jacho 341 ....................
2985 kaklik 342 .................... //------------------------------
343 .................... int8 hmc5883l_read_reg(int8 reg)
2926 jacho 344 .................... {
2985 kaklik 345 .................... int8 retval;
2926 jacho 346 ....................
2985 kaklik 347 .................... i2c_start();
348 .................... i2c_write(HMC5883L_WRT_ADDR);
349 .................... i2c_write(reg);
350 .................... i2c_start();
351 .................... i2c_write(HMC5883L_READ_ADDR);
352 .................... retval = i2c_read(0);
353 .................... i2c_stop();
2926 jacho 354 ....................
2985 kaklik 355 .................... return(retval);
2926 jacho 356 .................... }
357 ....................
2985 kaklik 358 .................... //------------------------------
359 .................... typedef struct
360 .................... {
361 .................... signed int16 x;
362 .................... signed int16 y;
363 .................... signed int16 z;
364 .................... }hmc5883l_result;
2926 jacho 365 ....................
2985 kaklik 366 .................... // This global structure holds the values read
367 .................... // from the HMC5883L x,y,z registers.
368 .................... hmc5883l_result compass = {0,0,0};
2926 jacho 369 *
2998 kaklik 370 0287: CLRF 21
371 0288: CLRF 22
372 0289: CLRF 23
373 028A: CLRF 24
374 028B: CLRF 25
375 028C: CLRF 26
2926 jacho 376 ....................
2985 kaklik 377 .................... //------------------------------
378 .................... void hmc5883l_read_data(void)
2926 jacho 379 .................... {
2985 kaklik 380 .................... unsigned int8 x_lsb;
381 .................... unsigned int8 x_msb;
2926 jacho 382 ....................
2985 kaklik 383 .................... unsigned int8 y_lsb;
384 .................... unsigned int8 y_msb;
2926 jacho 385 ....................
2985 kaklik 386 .................... unsigned int8 z_lsb;
387 .................... unsigned int8 z_msb;
2926 jacho 388 ....................
2985 kaklik 389 .................... i2c_start();
390 *
2998 kaklik 391 0151: BSF 20.4
392 0152: MOVF 20,W
393 0153: BSF 03.5
394 0154: MOVWF 07
395 0155: NOP
396 0156: BCF 03.5
397 0157: BSF 20.3
398 0158: MOVF 20,W
399 0159: BSF 03.5
400 015A: MOVWF 07
401 015B: NOP
402 015C: BCF 03.5
403 015D: BCF 07.4
404 015E: BCF 20.4
405 015F: MOVF 20,W
406 0160: BSF 03.5
407 0161: MOVWF 07
408 0162: NOP
409 0163: BCF 03.5
410 0164: BCF 07.3
411 0165: BCF 20.3
412 0166: MOVF 20,W
413 0167: BSF 03.5
414 0168: MOVWF 07
2985 kaklik 415 .................... i2c_write(HMC5883L_WRT_ADDR);
2998 kaklik 416 0169: MOVLW 3C
417 016A: BCF 03.5
418 016B: MOVWF 3D
419 016C: CALL 089
2985 kaklik 420 .................... i2c_write(HMC5883L_X_MSB_REG); // Point to X-msb register
2998 kaklik 421 016D: MOVLW 03
422 016E: MOVWF 3D
423 016F: CALL 089
2985 kaklik 424 .................... i2c_start();
2998 kaklik 425 0170: BSF 20.4
426 0171: MOVF 20,W
427 0172: BSF 03.5
428 0173: MOVWF 07
429 0174: NOP
430 0175: BCF 03.5
431 0176: BSF 20.3
432 0177: MOVF 20,W
433 0178: BSF 03.5
434 0179: MOVWF 07
435 017A: NOP
436 017B: BCF 03.5
437 017C: BTFSS 07.3
438 017D: GOTO 17C
439 017E: BCF 07.4
440 017F: BCF 20.4
441 0180: MOVF 20,W
442 0181: BSF 03.5
443 0182: MOVWF 07
444 0183: NOP
445 0184: BCF 03.5
446 0185: BCF 07.3
447 0186: BCF 20.3
448 0187: MOVF 20,W
449 0188: BSF 03.5
450 0189: MOVWF 07
2985 kaklik 451 .................... i2c_write(HMC5883L_READ_ADDR);
2998 kaklik 452 018A: MOVLW 3D
453 018B: BCF 03.5
454 018C: MOVWF 3D
455 018D: CALL 089
2926 jacho 456 ....................
2985 kaklik 457 .................... x_msb = i2c_read();
2998 kaklik 458 018E: MOVLW 01
459 018F: MOVWF 77
460 0190: CALL 10C
461 0191: MOVF 78,W
462 0192: MOVWF 38
2985 kaklik 463 .................... x_lsb = i2c_read();
2998 kaklik 464 0193: MOVLW 01
465 0194: MOVWF 77
466 0195: CALL 10C
467 0196: MOVF 78,W
468 0197: MOVWF 37
2926 jacho 469 ....................
2985 kaklik 470 .................... z_msb = i2c_read();
2998 kaklik 471 0198: MOVLW 01
472 0199: MOVWF 77
473 019A: CALL 10C
474 019B: MOVF 78,W
475 019C: MOVWF 3C
2985 kaklik 476 .................... z_lsb = i2c_read();
2998 kaklik 477 019D: MOVLW 01
478 019E: MOVWF 77
479 019F: CALL 10C
480 01A0: MOVF 78,W
481 01A1: MOVWF 3B
2926 jacho 482 ....................
2985 kaklik 483 .................... y_msb = i2c_read();
2998 kaklik 484 01A2: MOVLW 01
485 01A3: MOVWF 77
486 01A4: CALL 10C
487 01A5: MOVF 78,W
488 01A6: MOVWF 3A
2985 kaklik 489 .................... y_lsb = i2c_read(0); // do a NACK on last read
2998 kaklik 490 01A7: CLRF 77
491 01A8: CALL 10C
492 01A9: MOVF 78,W
493 01AA: MOVWF 39
2985 kaklik 494 ....................
495 .................... i2c_stop();
2998 kaklik 496 01AB: BCF 20.4
497 01AC: MOVF 20,W
498 01AD: BSF 03.5
499 01AE: MOVWF 07
500 01AF: NOP
501 01B0: BCF 03.5
502 01B1: BSF 20.3
503 01B2: MOVF 20,W
504 01B3: BSF 03.5
505 01B4: MOVWF 07
506 01B5: BCF 03.5
507 01B6: BTFSS 07.3
508 01B7: GOTO 1B6
509 01B8: NOP
510 01B9: GOTO 1BA
511 01BA: NOP
512 01BB: BSF 20.4
513 01BC: MOVF 20,W
514 01BD: BSF 03.5
515 01BE: MOVWF 07
516 01BF: NOP
2985 kaklik 517 ....................
518 .................... // Combine high and low bytes into 16-bit values.
519 .................... compass.x = make16(x_msb, x_lsb);
2998 kaklik 520 01C0: BCF 03.5
521 01C1: MOVF 38,W
522 01C2: MOVWF 22
523 01C3: MOVF 37,W
524 01C4: MOVWF 21
2985 kaklik 525 .................... compass.y = make16(y_msb, y_lsb);
2998 kaklik 526 01C5: MOVF 3A,W
527 01C6: MOVWF 24
528 01C7: MOVF 39,W
529 01C8: MOVWF 23
2985 kaklik 530 .................... compass.z = make16(z_msb, z_lsb);
2998 kaklik 531 01C9: MOVF 3C,W
532 01CA: MOVWF 26
533 01CB: MOVF 3B,W
534 01CC: MOVWF 25
2926 jacho 535 .................... }
2998 kaklik 536 01CD: BCF 0A.3
537 01CE: BCF 0A.4
538 01CF: GOTO 2F6 (RETURN)
2926 jacho 539 ....................
540 ....................
541 ....................
542 ....................
2988 kaklik 543 .................... #include <math.h>
544 .................... ////////////////////////////////////////////////////////////////////////////
545 .................... //// (C) Copyright 1996,2008 Custom Computer Services ////
546 .................... //// 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 ////
548 .................... //// licensed users of the CCS C compiler. No other use, reproduction ////
549 .................... //// or distribution is permitted without written permission. ////
550 .................... //// Derivative programs created using this software in object code ////
551 .................... //// form are not restricted in any way. ////
552 .................... ////////////////////////////////////////////////////////////////////////////
553 .................... //// ////
554 .................... //// History: ////
555 .................... //// * 9/20/2001 : Improvments are made to sin/cos code. ////
556 .................... //// The code now is small, much faster, ////
557 .................... //// and more accurate. ////
558 .................... //// * 2/21/2007 : Compiler handles & operator differently and does
559 .................... //// not return generic (int8 *) so type cast is done ////
560 .................... //// ////
561 .................... ////////////////////////////////////////////////////////////////////////////
2926 jacho 562 ....................
2988 kaklik 563 .................... #ifndef MATH_H
564 .................... #define MATH_H
565 ....................
566 .................... #ifdef PI
567 .................... #undef PI
568 .................... #endif
569 .................... #define PI 3.1415926535897932
570 ....................
571 ....................
572 .................... #define SQRT2 1.4142135623730950
573 ....................
574 .................... //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};
576 ....................
577 .................... ///////////////////////////// Round Functions //////////////////////////////
578 ....................
579 .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n)
580 .................... {
581 .................... float32 y, res;
582 .................... unsigned int16 l;
583 .................... int1 s;
584 ....................
585 .................... s = 0;
586 .................... y = x;
587 ....................
588 .................... if (x < 0)
589 .................... {
590 .................... s = 1;
591 .................... y = -y;
592 .................... }
593 ....................
594 .................... if (y <= 32768.0)
595 .................... res = (float32)(unsigned int16)y;
596 ....................
597 .................... else if (y < 10000000.0)
598 .................... {
599 .................... l = (unsigned int16)(y/32768.0);
600 .................... y = 32768.0*(y/32768.0 - (float32)l);
601 .................... res = 32768.0*(float32)l;
602 .................... res += (float32)(unsigned int16)y;
603 .................... }
604 ....................
605 .................... else
606 .................... res = y;
607 ....................
608 .................... y = y - (float32)(unsigned int16)y;
609 ....................
610 .................... if (s)
611 .................... res = -res;
612 ....................
613 .................... if (y != 0)
614 .................... {
615 .................... if (s == 1 && n == 0)
616 .................... res -= 1.0;
617 ....................
618 .................... if (s == 0 && n == 1)
619 .................... res += 1.0;
620 .................... }
621 .................... if (x == 0)
622 .................... res = 0;
623 ....................
624 .................... return (res);
625 .................... }
626 ....................
627 .................... // Overloaded Functions to take care for new Data types in PCD
628 .................... // Overloaded function CEIL_FLOOR() for data type - Float48
629 .................... #if defined(__PCD__)
630 .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n)
631 .................... {
632 .................... float48 y, res;
633 .................... unsigned int16 l;
634 .................... int1 s;
635 ....................
636 .................... s = 0;
637 .................... y = x;
638 ....................
639 .................... if (x < 0)
640 .................... {
641 .................... s = 1;
642 .................... y = -y;
643 .................... }
644 ....................
645 .................... if (y <= 32768.0)
646 .................... res = (float48)(unsigned int16)y;
647 ....................
648 .................... else if (y < 10000000.0)
649 .................... {
650 .................... l = (unsigned int16)(y/32768.0);
651 .................... y = 32768.0*(y/32768.0 - (float48)l);
652 .................... res = 32768.0*(float32)l;
653 .................... res += (float48)(unsigned int16)y;
654 .................... }
655 ....................
656 .................... else
657 .................... res = y;
658 ....................
659 .................... y = y - (float48)(unsigned int16)y;
660 ....................
661 .................... if (s)
662 .................... res = -res;
663 ....................
664 .................... if (y != 0)
665 .................... {
666 .................... if (s == 1 && n == 0)
667 .................... res -= 1.0;
668 ....................
669 .................... if (s == 0 && n == 1)
670 .................... res += 1.0;
671 .................... }
672 .................... if (x == 0)
673 .................... res = 0;
674 ....................
675 .................... return (res);
676 .................... }
677 ....................
678 ....................
679 .................... // Overloaded function CEIL_FLOOR() for data type - Float64
680 .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n)
681 .................... {
682 .................... float64 y, res;
683 .................... unsigned int16 l;
684 .................... int1 s;
685 ....................
686 .................... s = 0;
687 .................... y = x;
688 ....................
689 .................... if (x < 0)
690 .................... {
691 .................... s = 1;
692 .................... y = -y;
693 .................... }
694 ....................
695 .................... if (y <= 32768.0)
696 .................... res = (float64)(unsigned int16)y;
697 ....................
698 .................... else if (y < 10000000.0)
699 .................... {
700 .................... l = (unsigned int16)(y/32768.0);
701 .................... y = 32768.0*(y/32768.0 - (float64)l);
702 .................... res = 32768.0*(float64)l;
703 .................... res += (float64)(unsigned int16)y;
704 .................... }
705 ....................
706 .................... else
707 .................... res = y;
708 ....................
709 .................... y = y - (float64)(unsigned int16)y;
710 ....................
711 .................... if (s)
712 .................... res = -res;
713 ....................
714 .................... if (y != 0)
715 .................... {
716 .................... if (s == 1 && n == 0)
717 .................... res -= 1.0;
718 ....................
719 .................... if (s == 0 && n == 1)
720 .................... res += 1.0;
721 .................... }
722 .................... if (x == 0)
723 .................... res = 0;
724 ....................
725 .................... return (res);
726 .................... }
727 .................... #endif
728 ....................
729 .................... ////////////////////////////////////////////////////////////////////////////
730 .................... // float floor(float x)
731 .................... ////////////////////////////////////////////////////////////////////////////
732 .................... // Description : rounds down the number x.
733 .................... // Date : N/A
734 .................... //
735 .................... float32 floor(float32 x)
736 .................... {
737 .................... return CEIL_FLOOR(x, 0);
738 .................... }
739 .................... // Following 2 functions are overloaded functions of floor() for PCD
740 .................... // Overloaded function floor() for data type - Float48
741 .................... #if defined(__PCD__)
742 .................... float48 floor(float48 x)
743 .................... {
744 .................... return CEIL_FLOOR(x, 0);
745 .................... }
746 ....................
747 .................... // Overloaded function floor() for data type - Float64
748 .................... float64 floor(float64 x)
749 .................... {
750 .................... return CEIL_FLOOR(x, 0);
751 .................... }
752 .................... #endif
753 ....................
754 ....................
755 .................... ////////////////////////////////////////////////////////////////////////////
756 .................... // float ceil(float x)
757 .................... ////////////////////////////////////////////////////////////////////////////
758 .................... // Description : rounds up the number x.
759 .................... // Date : N/A
760 .................... //
761 .................... float32 ceil(float32 x)
762 .................... {
763 .................... return CEIL_FLOOR(x, 1);
764 .................... }
765 .................... // Following 2 functions are overloaded functions of ceil() for PCD
766 .................... // Overloaded function ceil() for data type - Float48
767 .................... #if defined(__PCD__)
768 .................... float48 ceil(float48 x)
769 .................... {
770 .................... return CEIL_FLOOR(x, 1);
771 .................... }
772 ....................
773 .................... // Overloaded function ceil() for data type - Float64
774 .................... float64 ceil(float64 x)
775 .................... {
776 .................... return CEIL_FLOOR(x, 1);
777 .................... }
778 .................... #endif
779 ....................
780 .................... ////////////////////////////////////////////////////////////////////////////
781 .................... // float fabs(float x)
782 .................... ////////////////////////////////////////////////////////////////////////////
783 .................... // Description : Computes the absolute value of floating point number x
784 .................... // Returns : returns the absolute value of x
785 .................... // Date : N/A
786 .................... //
787 .................... #define fabs abs
788 ....................
789 .................... ////////////////////////////////////////////////////////////////////////////
790 .................... // float fmod(float x)
791 .................... ////////////////////////////////////////////////////////////////////////////
792 .................... // 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
794 .................... // 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.
796 .................... // Date : N/A
797 .................... //
798 ....................
799 .................... float fmod(float32 x,float32 y)
800 .................... {
801 .................... float32 i;
802 .................... if (y!=0.0)
803 .................... {
804 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
805 .................... return(x-(i*y));
806 .................... }
807 .................... else
808 .................... {
809 .................... #ifdef _ERRNO
810 .................... {
811 .................... errno=EDOM;
812 .................... }
813 .................... #endif
814 .................... }
815 .................... }
816 .................... //Overloaded function for fmod() for PCD
817 .................... // Overloaded function fmod() for data type - Float48
818 .................... #if defined(__PCD__)
819 .................... float48 fmod(float48 x,float48 y)
820 .................... {
821 .................... float48 i;
822 .................... if (y!=0.0)
823 .................... {
824 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
825 .................... return(x-(i*y));
826 .................... }
827 .................... else
828 .................... {
829 .................... #ifdef _ERRNO
830 .................... {
831 .................... errno=EDOM;
832 .................... }
833 .................... #endif
834 .................... }
835 .................... }
836 .................... // Overloaded function fmod() for data type - Float64
837 .................... float64 fmod(float64 x,float64 y)
838 .................... {
839 .................... float64 i;
840 .................... if (y!=0.0)
841 .................... {
842 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
843 .................... return(x-(i*y));
844 .................... }
845 .................... else
846 .................... {
847 .................... #ifdef _ERRNO
848 .................... {
849 .................... errno=EDOM;
850 .................... }
851 .................... #endif
852 .................... }
853 .................... }
854 .................... #endif
855 .................... //////////////////// Exponential and logarithmic functions ////////////////////
856 .................... ////////////////////////////////////////////////////////////////////////////
857 .................... // float exp(float x)
858 .................... ////////////////////////////////////////////////////////////////////////////
859 .................... // Description : returns the value (e^x)
860 .................... // Date : N/A
861 .................... //
862 .................... #define LN2 0.6931471805599453
863 ....................
864 .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093,
865 .................... 0.0554965651, 0.240227138, 0.693147172};
866 ....................
867 ....................
868 .................... float32 exp(float32 x)
869 .................... {
870 .................... float32 y, res, r;
871 .................... #if defined(__PCD__)
872 .................... int8 data1;
873 .................... #endif
874 .................... signed int8 n;
875 .................... int1 s;
876 .................... #ifdef _ERRNO
877 .................... if(x > 88.722838)
878 .................... {
879 .................... errno=ERANGE;
880 .................... return(0);
881 .................... }
882 .................... #endif
883 .................... n = (signed int16)(x/LN2);
884 .................... s = 0;
885 .................... y = x;
886 ....................
887 .................... if (x < 0)
888 .................... {
889 .................... s = 1;
890 .................... n = -n;
891 .................... y = -y;
892 .................... }
893 ....................
894 .................... res = 0.0;
895 .................... #if !defined(__PCD__)
896 .................... *((unsigned int8 *)(&res)) = n + 0x7F;
897 .................... #endif
898 ....................
899 .................... #if defined(__PCD__) // Takes care of IEEE format for PCD
900 .................... data1 = n+0x7F;
901 .................... if(bit_test(data1,0))
902 .................... bit_set(*(((unsigned int8 *)(&res)+2)),7);
903 .................... rotate_right(&data1,1);
904 .................... bit_clear(data1,7);
905 .................... *(((unsigned int8 *)(&res)+3)) = data1;
906 .................... #endif
907 ....................
908 .................... y = y/LN2 - (float32)n;
909 ....................
910 .................... r = pe[0]*y + pe[1];
911 .................... r = r*y + pe[2];
912 .................... r = r*y + pe[3];
913 .................... r = r*y + pe[4];
914 .................... r = r*y + pe[5];
915 ....................
916 .................... res = res*(1.0 + y*r);
917 ....................
918 .................... if (s)
919 .................... res = 1.0/res;
920 .................... return(res);
921 .................... }
922 ....................
923 ....................
924 .................... //Overloaded function for exp() for PCD
925 .................... // Overloaded function exp() for data type - Float48
926 .................... #if defined(__PCD__)
927 .................... float48 exp(float48 x)
928 .................... {
929 .................... float48 y, res, r;
930 .................... int8 data1;
931 .................... signed int8 n;
932 .................... int1 s;
933 .................... #ifdef _ERRNO
934 .................... if(x > 88.722838)
935 .................... {
936 .................... errno=ERANGE;
937 .................... return(0);
938 .................... }
939 .................... #endif
940 .................... n = (signed int16)(x/LN2);
941 .................... s = 0;
942 .................... y = x;
943 ....................
944 .................... if (x < 0)
945 .................... {
946 .................... s = 1;
947 .................... n = -n;
948 .................... y = -y;
949 .................... }
950 ....................
951 .................... res = 0.0;
952 ....................
953 .................... data1 = n+0x7F;
954 .................... if(bit_test(data1,0))
955 .................... bit_set(*(((unsigned int8 *)(&res)+4)),7);
956 .................... rotate_right(&data1,1);
957 .................... bit_clear(data1,7);
958 .................... *(((unsigned int8 *)(&res)+5)) = data1;
959 ....................
960 .................... y = y/LN2 - (float48)n;
961 ....................
962 .................... r = pe[0]*y + pe[1];
963 .................... r = r*y + pe[2];
964 .................... r = r*y + pe[3];
965 .................... r = r*y + pe[4];
966 .................... r = r*y + pe[5];
967 ....................
968 .................... res = res*(1.0 + y*r);
969 ....................
970 .................... if (s)
971 .................... res = 1.0/res;
972 .................... return(res);
973 .................... }
974 ....................
975 .................... // Overloaded function exp() for data type - Float64
976 .................... float64 exp(float64 x)
977 .................... {
978 .................... float64 y, res, r;
979 .................... unsigned int16 data1, data2;
980 .................... unsigned int16 *p;
981 .................... signed int16 n;
982 .................... int1 s;
983 .................... #ifdef _ERRNO
984 .................... if(x > 709.7827128)
985 .................... {
986 .................... errno=ERANGE;
987 .................... return(0);
988 .................... }
989 .................... #endif
990 .................... n = (signed int16)(x/LN2);
991 .................... s = 0;
992 .................... y = x;
993 ....................
994 .................... if (x < 0)
995 .................... {
996 .................... s = 1;
997 .................... n = -n;
998 .................... y = -y;
999 .................... }
1000 ....................
1001 .................... res = 0.0;
1002 ....................
1003 .................... #if !defined(__PCD__)
1004 .................... *((unsigned int16 *)(&res)) = n + 0x7F;
1005 .................... #endif
1006 .................... p= (((unsigned int16 *)(&res))+3);
1007 .................... data1 = *p;
1008 .................... data2 = *p;
1009 .................... data1 = n + 0x3FF;
1010 .................... data1 = data1 <<4;
1011 .................... if(bit_test(data2,15))
1012 .................... bit_set(data1,15);
1013 .................... data2 = data2 & 0x000F;
1014 .................... data1 ^= data2;
1015 ....................
1016 .................... *(((unsigned int16 *)(&res)+3)) = data1;
1017 ....................
1018 ....................
1019 .................... y = y/LN2 - (float64)n;
1020 ....................
1021 .................... r = pe[0]*y + pe[1];
1022 .................... r = r*y + pe[2];
1023 .................... r = r*y + pe[3];
1024 .................... r = r*y + pe[4];
1025 .................... r = r*y + pe[5];
1026 ....................
1027 .................... res = res*(1.0 + y*r);
1028 ....................
1029 .................... if (s)
1030 .................... res = 1.0/res;
1031 .................... return(res);
1032 .................... }
1033 ....................
1034 .................... #ENDIF
1035 ....................
1036 ....................
1037 .................... /************************************************************/
1038 ....................
1039 .................... 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};
1041 ....................
1042 .................... ////////////////////////////////////////////////////////////////////////////
1043 .................... // float log(float x)
1044 .................... ////////////////////////////////////////////////////////////////////////////
1045 .................... // Description : returns the the natural log of x
1046 .................... // Date : N/A
1047 .................... //
1048 .................... float32 log(float32 x)
1049 .................... {
1050 .................... float32 y, res, r, y2;
1051 .................... #if defined(__PCD__)
1052 .................... unsigned int8 data1,data2;
1053 .................... #endif
1054 .................... signed int8 n;
1055 .................... #ifdef _ERRNO
1056 .................... if(x <0)
1057 .................... {
1058 .................... errno=EDOM;
1059 .................... }
1060 .................... if(x ==0)
1061 .................... {
1062 .................... errno=ERANGE;
1063 .................... return(0);
1064 .................... }
1065 .................... #endif
1066 .................... y = x;
1067 ....................
1068 .................... if (y != 1.0)
1069 .................... {
1070 .................... #if !defined(__PCD__)
1071 .................... *((unsigned int8 *)(&y)) = 0x7E;
1072 .................... #endif
1073 ....................
1074 .................... #if defined(__PCD__) // Takes care of IEEE format
1075 .................... data2 = *(((unsigned int8 *)(&y))+3);
1076 .................... *(((unsigned int8 *)(&y))+3) = 0x3F;
1077 .................... data1 = *(((unsigned int8 *)(&y))+2);
1078 .................... bit_clear(data1,7);
1079 .................... *(((unsigned int8 *)(&y))+2) = data1;
1080 .................... if(bit_test(data2,7))
1081 .................... bit_set(*(((unsigned int8 *)(&y))+3),7);
1082 .................... #endif
1083 ....................
1084 .................... y = (y - 1.0)/(y + 1.0);
1085 ....................
1086 .................... y2=y*y;
1087 ....................
1088 .................... res = pl[0]*y2 + pl[1];
1089 .................... res = res*y2 + pl[2];
1090 .................... res = res*y2 + pl[3];
1091 ....................
1092 .................... r = ql[0]*y2 + ql[1];
1093 .................... r = r*y2 + ql[2];
1094 .................... r = r*y2 + ql[3];
1095 ....................
1096 .................... res = y*res/r;
1097 .................... #if !defined(__PCD__)
1098 .................... n = *((unsigned int8 *)(&x)) - 0x7E;
1099 .................... #endif
1100 .................... #if defined(__PCD__)
1101 .................... data1 = *(((unsigned int8 *)(&x)+3));
1102 .................... rotate_left(&data1,1);
1103 .................... data2 = *(((unsigned int8 *)(&x)+2));
1104 .................... if(bit_test (data2,7))
1105 .................... bit_set(data1,0);
1106 .................... n = data1 - 0x7E;
1107 .................... #endif
1108 ....................
1109 .................... if (n<0)
1110 .................... r = -(float32)-n;
1111 .................... else
1112 .................... r = (float32)n;
1113 ....................
1114 .................... res += r*LN2;
1115 .................... }
1116 ....................
1117 .................... else
1118 .................... res = 0.0;
1119 ....................
1120 .................... return(res);
1121 .................... }
1122 ....................
1123 .................... //Overloaded function for log() for PCD
1124 .................... // Overloaded function log() for data type - Float48
1125 .................... #if defined(__PCD__)
1126 .................... float48 log(float48 x)
1127 .................... {
1128 .................... float48 y, res, r, y2;
1129 .................... unsigned int8 data1,data2;
1130 .................... signed int8 n;
1131 .................... #ifdef _ERRNO
1132 .................... if(x <0)
1133 .................... {
1134 .................... errno=EDOM;
1135 .................... }
1136 .................... if(x ==0)
1137 .................... {
1138 .................... errno=ERANGE;
1139 .................... return(0);
1140 .................... }
1141 .................... #endif
1142 .................... y = x;
1143 ....................
1144 .................... if (y != 1.0)
1145 .................... {
1146 ....................
1147 .................... #if !defined(__PCD__)
1148 .................... *((unsigned int8 *)(&y)) = 0x7E;
1149 .................... #endif
1150 .................... data2 = *(((unsigned int8 *)(&y))+5);
1151 .................... *(((unsigned int8 *)(&y))+5) = 0x3F;
1152 .................... data1 = *(((unsigned int8 *)(&y))+4);
1153 .................... bit_clear(data1,7);
1154 .................... *(((unsigned int8 *)(&y))+4) = data1;
1155 ....................
1156 .................... if(bit_test(data2,7))
1157 .................... bit_set(*(((unsigned int8 *)(&y))+4),7);
1158 .................... y = (y - 1.0)/(y + 1.0);
1159 ....................
1160 .................... y2=y*y;
1161 ....................
1162 .................... res = pl[0]*y2 + pl[1];
1163 .................... res = res*y2 + pl[2];
1164 .................... res = res*y2 + pl[3];
1165 ....................
1166 .................... r = ql[0]*y2 + ql[1];
1167 .................... r = r*y2 + ql[2];
1168 .................... r = r*y2 + ql[3];
1169 ....................
1170 .................... res = y*res/r;
1171 ....................
1172 .................... data1 = *(((unsigned int8 *)(&x)+5));
1173 .................... rotate_left(&data1,1);
1174 .................... data2 = *(((unsigned int8 *)(&x)+4));
1175 .................... if(bit_test (data2,7))
1176 .................... bit_set(data1,0);
1177 ....................
1178 .................... n = data1 - 0x7E;
1179 ....................
1180 .................... if (n<0)
1181 .................... r = -(float48)-n;
1182 .................... else
1183 .................... r = (float48)n;
1184 ....................
1185 .................... res += r*LN2;
1186 .................... }
1187 ....................
1188 .................... else
1189 .................... res = 0.0;
1190 ....................
1191 .................... return(res);
1192 .................... }
1193 ....................
1194 .................... // Overloaded function log() for data type - Float48
1195 .................... #if defined(__PCD__)
1196 .................... 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};
1198 .................... #endif
1199 .................... float64 log(float64 x)
1200 .................... {
1201 .................... float64 y, res, r, y2;
1202 .................... unsigned int16 data1,data2;
1203 .................... unsigned int16 *p;
1204 .................... signed int16 n;
1205 .................... #ifdef _ERRNO
1206 .................... if(x <0)
1207 .................... {
1208 .................... errno=EDOM;
1209 .................... }
1210 .................... if(x ==0)
1211 .................... {
1212 .................... errno=ERANGE;
1213 .................... return(0);
1214 .................... }
1215 .................... #endif
1216 .................... y = x;
1217 ....................
1218 .................... if (y != 1.0)
1219 .................... {
1220 .................... #if !defined(__PCD__)
1221 .................... *((unsigned int8 *)(&y)) = 0x7E;
1222 .................... #endif
1223 .................... p= (((unsigned int16 *)(&y))+3);
1224 .................... data1 = *p;
1225 .................... data2 = *p;
1226 .................... data1 = 0x3FE;
1227 .................... data1 = data1 <<4;
1228 .................... if(bit_test (data2,15))
1229 .................... bit_set(data1,15);
1230 .................... data2 = data2 & 0x000F;
1231 .................... data1 ^=data2;
1232 ....................
1233 .................... *p = data1;
1234 ....................
1235 .................... y = (y - 1.0)/(y + 1.0);
1236 ....................
1237 .................... y2=y*y;
1238 ....................
1239 .................... res = pl_64[0]*y2 + pl_64[1];
1240 .................... res = res*y2 + pl_64[2];
1241 .................... res = res*y2 + pl_64[3];
1242 ....................
1243 .................... r = ql_64[0]*y2 + ql_64[1];
1244 .................... r = r*y2 + ql_64[2];
1245 .................... r = r*y2 + ql_64[3];
1246 ....................
1247 .................... res = y*res/r;
1248 ....................
1249 .................... p= (((unsigned int16 *)(&x))+3);
1250 .................... data1 = *p;
1251 .................... bit_clear(data1,15);
1252 .................... data1 = data1 >>4;
1253 .................... n = data1 - 0x3FE;
1254 ....................
1255 ....................
1256 .................... if (n<0)
1257 .................... r = -(float64)-n;
1258 .................... else
1259 .................... r = (float64)n;
1260 ....................
1261 .................... res += r*LN2;
1262 .................... }
1263 ....................
1264 .................... else
1265 .................... res = 0.0;
1266 ....................
1267 .................... return(res);
1268 .................... }
1269 .................... #endif
1270 ....................
1271 ....................
1272 .................... #define LN10 2.3025850929940456
1273 ....................
1274 .................... ////////////////////////////////////////////////////////////////////////////
1275 .................... // float log10(float x)
1276 .................... ////////////////////////////////////////////////////////////////////////////
1277 .................... // Description : returns the the log base 10 of x
1278 .................... // Date : N/A
1279 .................... //
1280 .................... float32 log10(float32 x)
1281 .................... {
1282 .................... float32 r;
1283 ....................
1284 .................... r = log(x);
1285 .................... r = r/LN10;
1286 .................... return(r);
1287 .................... }
1288 ....................
1289 .................... //Overloaded functions for log10() for PCD
1290 .................... // Overloaded function log10() for data type - Float48
1291 .................... #if defined(__PCD__)
1292 .................... float48 log10(float48 x)
1293 .................... {
1294 .................... float48 r;
1295 ....................
1296 .................... r = log(x);
1297 .................... r = r/LN10;
1298 .................... return(r);
1299 .................... }
1300 ....................
1301 .................... // Overloaded function log10() for data type - Float64
1302 .................... float64 log10(float64 x)
1303 .................... {
1304 .................... float64 r;
1305 ....................
1306 .................... r = log(x);
1307 .................... r = r/LN10;
1308 .................... return(r);
1309 .................... }
1310 .................... #endif
1311 .................... ////////////////////////////////////////////////////////////////////////////
1312 .................... // float modf(float x)
1313 .................... ////////////////////////////////////////////////////////////////////////////
1314 .................... // 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
1316 .................... // as a float in the object pointed to by the iptr
1317 .................... // Returns : returns the signed fractional part of value.
1318 .................... // Date : N/A
1319 .................... //
1320 ....................
1321 .................... float32 modf(float32 value,float32 *iptr)
1322 .................... {
1323 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1324 .................... return(value - *iptr);
1325 .................... }
1326 .................... //Overloaded functions for modf() for PCD
1327 .................... // Overloaded function modf() for data type - Float48
1328 .................... #if defined(__PCD__)
1329 .................... float48 modf(float48 value,float48 *iptr)
1330 .................... {
1331 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1332 .................... return(value - *iptr);
1333 .................... }
1334 .................... // Overloaded function modf() for data type - Float64
1335 .................... float64 modf(float64 value,float64 *iptr)
1336 .................... {
1337 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1338 .................... return(value - *iptr);
1339 .................... }
1340 .................... #endif
1341 ....................
1342 .................... ////////////////////////////////////////////////////////////////////////////
1343 .................... // float pwr(float x,float y)
1344 .................... ////////////////////////////////////////////////////////////////////////////
1345 .................... // Description : returns the value (x^y)
1346 .................... // Date : N/A
1347 .................... // Note : 0 is returned when the function will generate an imaginary number
1348 .................... //
1349 .................... float32 pwr(float32 x,float32 y)
1350 .................... {
1351 .................... if(0 > x && fmod(y, 1) == 0) {
1352 .................... if(fmod(y, 2) == 0) {
1353 .................... return (exp(log(-x) * y));
1354 .................... } else {
1355 .................... return (-exp(log(-x) * y));
1356 .................... }
1357 .................... } else if(0 > x && fmod(y, 1) != 0) {
1358 .................... return 0;
1359 .................... } else {
1360 .................... if(x != 0 || 0 >= y) {
1361 .................... return (exp(log(x) * y));
1362 .................... }
1363 .................... }
1364 .................... }
1365 .................... //Overloaded functions for pwr() for PCD
1366 .................... // Overloaded function pwr() for data type - Float48
1367 .................... #if defined(__PCD__)
1368 .................... float48 pwr(float48 x,float48 y)
1369 .................... {
1370 .................... if(0 > x && fmod(y, 1) == 0) {
1371 .................... if(fmod(y, 2) == 0) {
1372 .................... return (exp(log(-x) * y));
1373 .................... } else {
1374 .................... return (-exp(log(-x) * y));
1375 .................... }
1376 .................... } else if(0 > x && fmod(y, 1) != 0) {
1377 .................... return 0;
1378 .................... } else {
1379 .................... if(x != 0 || 0 >= y) {
1380 .................... return (exp(log(x) * y));
1381 .................... }
1382 .................... }
1383 .................... }
1384 .................... // Overloaded function pwr() for data type - Float64
1385 .................... float64 pwr(float64 x,float64 y)
1386 .................... {
1387 .................... if(0 > x && fmod(y, 1) == 0) {
1388 .................... if(fmod(y, 2) == 0) {
1389 .................... return (exp(log(-x) * y));
1390 .................... } else {
1391 .................... return (-exp(log(-x) * y));
1392 .................... }
1393 .................... } else if(0 > x && fmod(y, 1) != 0) {
1394 .................... return 0;
1395 .................... } else {
1396 .................... if(x != 0 || 0 >= y) {
1397 .................... return (exp(log(x) * y));
1398 .................... }
1399 .................... }
1400 .................... }
1401 .................... #endif
1402 ....................
1403 .................... //////////////////// Power functions ////////////////////
1404 ....................
1405 .................... ////////////////////////////////////////////////////////////////////////////
1406 .................... // float pow(float x,float y)
1407 .................... ////////////////////////////////////////////////////////////////////////////
1408 .................... // Description : returns the value (x^y)
1409 .................... // Date : N/A
1410 .................... // Note : 0 is returned when the function will generate an imaginary number
1411 .................... //
1412 .................... float32 pow(float32 x,float32 y)
1413 .................... {
1414 .................... if(0 > x && fmod(y, 1) == 0) {
1415 .................... if(fmod(y, 2) == 0) {
1416 .................... return (exp(log(-x) * y));
1417 .................... } else {
1418 .................... return (-exp(log(-x) * y));
1419 .................... }
1420 .................... } else if(0 > x && fmod(y, 1) != 0) {
1421 .................... return 0;
1422 .................... } else {
1423 .................... if(x != 0 || 0 >= y) {
1424 .................... return (exp(log(x) * y));
1425 .................... }
1426 .................... }
1427 .................... }
1428 .................... //Overloaded functions for pow() for PCD
1429 .................... // Overloaded function for pow() data type - Float48
1430 .................... #if defined(__PCD__)
1431 .................... float48 pow(float48 x,float48 y)
1432 .................... {
1433 .................... if(0 > x && fmod(y, 1) == 0) {
1434 .................... if(fmod(y, 2) == 0) {
1435 .................... return (exp(log(-x) * y));
1436 .................... } else {
1437 .................... return (-exp(log(-x) * y));
1438 .................... }
1439 .................... } else if(0 > x && fmod(y, 1) != 0) {
1440 .................... return 0;
1441 .................... } else {
1442 .................... if(x != 0 || 0 >= y) {
1443 .................... return (exp(log(x) * y));
1444 .................... }
1445 .................... }
1446 .................... }
1447 ....................
1448 .................... // Overloaded function pow() for data type - Float64
1449 .................... float64 pow(float64 x,float64 y)
1450 .................... {
1451 .................... if(0 > x && fmod(y, 1) == 0) {
1452 .................... if(fmod(y, 2) == 0) {
1453 .................... return (exp(log(-x) * y));
1454 .................... } else {
1455 .................... return (-exp(log(-x) * y));
1456 .................... }
1457 .................... } else if(0 > x && fmod(y, 1) != 0) {
1458 .................... return 0;
1459 .................... } else {
1460 .................... if(x != 0 || 0 >= y) {
1461 .................... return (exp(log(x) * y));
1462 .................... }
1463 .................... }
1464 .................... }
1465 .................... #endif
1466 ....................
1467 .................... ////////////////////////////////////////////////////////////////////////////
1468 .................... // float sqrt(float x)
1469 .................... ////////////////////////////////////////////////////////////////////////////
1470 .................... // Description : returns the square root of x
1471 .................... // Date : N/A
1472 .................... //
1473 .................... float32 sqrt(float32 x)
1474 .................... {
1475 .................... float32 y, res;
1476 .................... #if defined(__PCD__)
1477 .................... unsigned int16 data1,data2;
1478 .................... #endif
1479 .................... BYTE *p;
1480 ....................
1481 .................... #ifdef _ERRNO
1482 .................... if(x < 0)
1483 .................... {
1484 .................... errno=EDOM;
1485 .................... }
1486 .................... #endif
1487 ....................
1488 .................... if( x<=0.0)
1489 .................... return(0.0);
1490 ....................
1491 .................... y=x;
1492 ....................
1493 .................... #if !defined(__PCD__)
1494 .................... p=&y;
1495 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1);
1496 .................... #endif
1497 ....................
1498 .................... #if defined(__PCD__)
1499 .................... p = (((unsigned int8 *)(&y))+3);
1500 .................... data1 = *(((unsigned int8 *)(&y))+3);
1501 .................... data2 = *(((unsigned int8 *)(&y))+2);
1502 .................... rotate_left(&data1,1);
1503 .................... if(bit_test(data2,7))
1504 .................... bit_set(data1,0);
1505 .................... data1 = ((data1+127) >>1);
1506 .................... bit_clear(data2,7);
1507 .................... if(bit_test(data1,0))
1508 .................... bit_set(data2,7);
1509 .................... data1 = data1 >>1;
1510 .................... *(((unsigned int8 *)(&y))+3) = data1;
1511 .................... *(((unsigned int8 *)(&y))+2) = data2;
1512 ....................
1513 .................... #endif
1514 ....................
1515 .................... do {
1516 .................... res=y;
1517 .................... y+=(x/y);
1518 ....................
1519 .................... #if !defined(__PCD__)
1520 .................... (*p)--;
1521 .................... #endif
1522 ....................
1523 .................... #if defined(__PCD__)
1524 .................... data1 = *(((unsigned int8 *)(&y))+3);
1525 .................... data2 = *(((unsigned int8 *)(&y))+2);
1526 .................... rotate_left(&data1,1);
1527 .................... if(bit_test(data2,7))
1528 .................... bit_set(data1,0);
1529 .................... data1--;
1530 .................... bit_clear(data2,7);
1531 .................... if(bit_test(data1,0))
1532 .................... bit_set(data2,7);
1533 .................... data1 = data1 >>1;
1534 .................... *(((unsigned int8 *)(&y))+3) = data1;
1535 .................... *(((unsigned int8 *)(&y))+2) = data2;
1536 ....................
1537 .................... #endif
1538 .................... } while(res != y);
1539 ....................
1540 .................... return(res);
1541 .................... }
1542 .................... //Overloaded functions for sqrt() for PCD
1543 .................... // Overloaded function sqrt() for data type - Float48
1544 .................... #if defined(__PCD__)
1545 .................... float48 sqrt(float48 x)
1546 .................... {
1547 .................... float48 y, res;
1548 .................... unsigned int16 data1,data2;
1549 .................... BYTE *p;
1550 ....................
1551 .................... #ifdef _ERRNO
1552 .................... if(x < 0)
1553 .................... {
1554 .................... errno=EDOM;
1555 .................... }
1556 .................... #endif
1557 ....................
1558 .................... if( x<=0.0)
1559 .................... return(0.0);
1560 ....................
1561 .................... y=x;
1562 ....................
1563 .................... #if !defined(__PCD__)
1564 .................... p=&y;
1565 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1);
1566 .................... #endif
1567 ....................
1568 .................... #if defined(__PCD__)
1569 .................... p = (((unsigned int8 *)(&y))+5);
1570 .................... data1 = *(((unsigned int8 *)(&y))+5);
1571 .................... data2 = *(((unsigned int8 *)(&y))+4);
1572 .................... rotate_left(&data1,1);
1573 .................... if(bit_test(data2,7))
1574 .................... bit_set(data1,0);
1575 .................... data1 = ((data1+127) >>1);
1576 .................... bit_clear(data2,7);
1577 .................... if(bit_test(data1,0))
1578 .................... bit_set(data2,7);
1579 .................... data1 = data1 >>1;
1580 .................... *(((unsigned int8 *)(&y))+5) = data1;
1581 .................... *(((unsigned int8 *)(&y))+4) = data2;
1582 ....................
1583 .................... #endif
1584 ....................
1585 .................... do {
1586 .................... res=y;
1587 .................... y+=(x/y);
1588 ....................
1589 .................... #if !defined(__PCD__)
1590 .................... (*p)--;
1591 .................... #endif
1592 ....................
1593 .................... data1 = *(((unsigned int8 *)(&y))+5);
1594 .................... data2 = *(((unsigned int8 *)(&y))+4);
1595 .................... rotate_left(&data1,1);
1596 .................... if(bit_test(data2,7))
1597 .................... bit_set(data1,0);
1598 .................... data1--;
1599 .................... bit_clear(data2,7);
1600 .................... if(bit_test(data1,0))
1601 .................... bit_set(data2,7);
1602 .................... data1 = data1 >>1;
1603 .................... *(((unsigned int8 *)(&y))+5) = data1;
1604 .................... *(((unsigned int8 *)(&y))+4) = data2;
1605 ....................
1606 .................... } while(res != y);
1607 ....................
1608 .................... return(res);
1609 .................... }
1610 ....................
1611 .................... // Overloaded function sqrt() for data type - Float64
1612 .................... float64 sqrt(float64 x)
1613 .................... {
1614 .................... float64 y, res;
1615 .................... unsigned int16 *p;
1616 .................... unsigned int16 temp1,temp2;
1617 ....................
1618 .................... #ifdef _ERRNO
1619 .................... if(x < 0)
1620 .................... {
1621 .................... errno=EDOM;
1622 .................... }
1623 .................... #endif
1624 ....................
1625 .................... if( x<=0.0)
1626 .................... return(0.0);
1627 ....................
1628 .................... y=x;
1629 .................... p= (((unsigned int16 *)(&y))+3);
1630 .................... temp1 = *p;
1631 .................... temp2 = *p;
1632 .................... bit_clear(temp1,15);
1633 .................... temp1 = (temp1>>4)+1023;
1634 .................... temp1 = temp1 >> 1;
1635 .................... temp1 = (temp1<<4) & 0xFFF0;
1636 .................... if(bit_test(temp2,15))
1637 .................... bit_set(temp1,15);
1638 .................... temp2 = temp2 & 0x000F;
1639 .................... temp1 ^= temp2;
1640 ....................
1641 .................... (*p) = temp1;
1642 ....................
1643 .................... do {
1644 .................... res=y;
1645 .................... y+=(x/y);
1646 .................... temp1 = *p;
1647 .................... temp2 = *p;
1648 .................... bit_clear(temp1,15);
1649 .................... temp1 = (temp1>>4);
1650 .................... temp1--;
1651 .................... temp1 = (temp1<<4) & 0xFFF0;
1652 .................... if(bit_test(temp2,15))
1653 .................... bit_set(temp1,15);
1654 .................... temp2 = temp2 & 0x000F;
1655 .................... temp1 ^= temp2;
1656 .................... (*p) = temp1;
1657 ....................
1658 .................... } while(res != y);
1659 ....................
1660 .................... return(res);
1661 .................... }
1662 .................... #endif
1663 ....................
1664 .................... ////////////////////////////// Trig Functions //////////////////////////////
1665 .................... #ifdef PI_DIV_BY_TWO
1666 .................... #undef PI_DIV_BY_TWO
1667 .................... #endif
1668 .................... #define PI_DIV_BY_TWO 1.5707963267948966
1669 .................... #ifdef TWOBYPI
1670 .................... #undef TWOBYPI
1671 .................... #define TWOBYPI 0.6366197723675813
1672 .................... #endif
1673 .................... ////////////////////////////////////////////////////////////////////////////
1674 .................... // float cos(float x)
1675 .................... ////////////////////////////////////////////////////////////////////////////
1676 .................... // Description : returns the cosine value of the angle x, which is in radian
1677 .................... // Date : 9/20/2001
1678 .................... //
1679 .................... float32 cos(float32 x)
1680 .................... {
1681 .................... float32 y, t, t2 = 1.0;
1682 .................... unsigned int8 quad, i;
1683 .................... float32 frac;
1684 .................... float32 p[6] = { //by the series definition for cosine
1685 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1686 .................... 0.04166666666667,
1687 .................... -0.00138888888889,
1688 .................... 0.00002480158730,
1689 .................... -0.00000027557319,
1690 .................... 0.00000000208767,
1691 .................... //-0.00000000001147,
1692 .................... // 0.00000000000005
1693 .................... };
1694 ....................
1695 .................... if (x < 0) x = -x; // absolute value of input
1696 ....................
1697 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1698 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1699 .................... quad = quad % 4; // quadrant (0 to 3)
1700 ....................
1701 .................... if (quad == 0 || quad == 2)
1702 .................... t = frac * PI_DIV_BY_TWO;
1703 .................... else if (quad == 1)
1704 .................... t = (1-frac) * PI_DIV_BY_TWO;
1705 .................... else // should be 3
1706 .................... t = (frac-1) * PI_DIV_BY_TWO;
1707 ....................
1708 .................... y = 1.0;
1709 .................... t = t * t;
1710 .................... for (i = 0; i <= 5; i++)
1711 .................... {
1712 .................... t2 = t2 * t;
1713 .................... y = y + p[i] * t2;
1714 .................... }
1715 ....................
1716 .................... if (quad == 2 || quad == 1)
1717 .................... y = -y; // correct sign
1718 ....................
1719 .................... return (y);
1720 .................... }
1721 ....................
1722 ....................
1723 .................... //Overloaded functions for cos() for PCD
1724 .................... // Overloaded function cos() for data type - Float48
1725 .................... #if defined(__PCD__)
1726 .................... float48 cos(float48 x)
1727 .................... {
1728 .................... float48 y, t, t2 = 1.0;
1729 .................... unsigned int8 quad, i;
1730 .................... float48 frac;
1731 .................... float48 p[6] = { //by the series definition for cosine
1732 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1733 .................... 0.04166666666667,
1734 .................... -0.00138888888889,
1735 .................... 0.00002480158730,
1736 .................... -0.00000027557319,
1737 .................... 0.00000000208767,
1738 .................... //-0.00000000001147,
1739 .................... // 0.00000000000005
1740 .................... };
1741 ....................
1742 .................... if (x < 0) x = -x; // absolute value of input
1743 ....................
1744 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1745 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1746 .................... quad = quad % 4; // quadrant (0 to 3)
1747 ....................
1748 .................... if (quad == 0 || quad == 2)
1749 .................... t = frac * PI_DIV_BY_TWO;
1750 .................... else if (quad == 1)
1751 .................... t = (1-frac) * PI_DIV_BY_TWO;
1752 .................... else // should be 3
1753 .................... t = (frac-1) * PI_DIV_BY_TWO;
1754 ....................
1755 .................... y = 0.999999999781;
1756 .................... t = t * t;
1757 .................... for (i = 0; i <= 5; i++)
1758 .................... {
1759 .................... t2 = t2 * t;
1760 .................... y = y + p[i] * t2;
1761 .................... }
1762 ....................
1763 .................... if (quad == 2 || quad == 1)
1764 .................... y = -y; // correct sign
1765 ....................
1766 .................... return (y);
1767 .................... }
1768 ....................
1769 .................... // Overloaded function cos() for data type - Float48
1770 .................... float64 cos(float64 x)
1771 .................... {
1772 .................... float64 y, t, t2 = 1.0;
1773 .................... unsigned int8 quad, i;
1774 .................... float64 frac;
1775 .................... float64 p[6] = { //by the series definition for cosine
1776 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1777 .................... 0.04166666666667,
1778 .................... -0.00138888888889,
1779 .................... 0.00002480158730,
1780 .................... -0.00000027557319,
1781 .................... 0.00000000208767,
1782 .................... //-0.00000000001147,
1783 .................... // 0.00000000000005
1784 .................... };
1785 ....................
1786 .................... if (x < 0) x = -x; // absolute value of input
1787 ....................
1788 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1789 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1790 .................... quad = quad % 4; // quadrant (0 to 3)
1791 ....................
1792 .................... if (quad == 0 || quad == 2)
1793 .................... t = frac * PI_DIV_BY_TWO;
1794 .................... else if (quad == 1)
1795 .................... t = (1-frac) * PI_DIV_BY_TWO;
1796 .................... else // should be 3
1797 .................... t = (frac-1) * PI_DIV_BY_TWO;
1798 ....................
1799 .................... y = 0.999999999781;
1800 .................... t = t * t;
1801 .................... for (i = 0; i <= 5; i++)
1802 .................... {
1803 .................... t2 = t2 * t;
1804 .................... y = y + p[i] * t2;
1805 .................... }
1806 ....................
1807 .................... if (quad == 2 || quad == 1)
1808 .................... y = -y; // correct sign
1809 ....................
1810 .................... return (y);
1811 .................... }
1812 ....................
1813 .................... #endif
1814 ....................
1815 .................... ////////////////////////////////////////////////////////////////////////////
1816 .................... // float sin(float x)
1817 .................... ////////////////////////////////////////////////////////////////////////////
1818 .................... // Description : returns the sine value of the angle x, which is in radian
1819 .................... // Date : 9/20/2001
1820 .................... //
1821 .................... float32 sin(float32 x)
1822 .................... {
1823 .................... return cos(x - PI_DIV_BY_TWO);
1824 .................... }
1825 ....................
1826 .................... //Overloaded functions for sin() for PCD
1827 .................... // Overloaded function sin() for data type - Float48
1828 .................... #if defined(__PCD__)
1829 .................... float48 sin(float48 x)
1830 .................... {
1831 .................... return cos(x - PI_DIV_BY_TWO);
1832 .................... }
1833 ....................
1834 .................... // Overloaded function sin() for data type - Float48
1835 .................... float64 sin(float64 x)
1836 .................... {
1837 .................... return cos(x - PI_DIV_BY_TWO);
1838 .................... }
1839 .................... #endif
1840 ....................
1841 .................... ////////////////////////////////////////////////////////////////////////////
1842 .................... // float tan(float x)
1843 .................... ////////////////////////////////////////////////////////////////////////////
1844 .................... // Description : returns the tangent value of the angle x, which is in radian
1845 .................... // Date : 9/20/2001
1846 .................... //
1847 .................... float32 tan(float32 x)
1848 .................... {
1849 .................... float32 c, s;
1850 ....................
1851 .................... c = cos(x);
1852 .................... if (c == 0.0)
1853 .................... return (1.0e+36);
1854 ....................
1855 .................... s = sin(x);
1856 .................... return(s/c);
1857 .................... }
1858 .................... //Overloaded functions for tan() for PCD
1859 .................... // Overloaded function tan() for data type - Float48
1860 .................... #if defined(__PCD__)
1861 .................... float48 tan(float48 x)
1862 .................... {
1863 .................... float48 c, s;
1864 ....................
1865 .................... c = cos(x);
1866 .................... if (c == 0.0)
1867 .................... return (1.0e+36);
1868 ....................
1869 .................... s = sin(x);
1870 .................... return(s/c);
1871 .................... }
1872 ....................
1873 .................... // Overloaded function tan() for data type - Float48
1874 .................... float64 tan(float64 x)
1875 .................... {
1876 .................... float64 c, s;
1877 ....................
1878 .................... c = cos(x);
1879 .................... if (c == 0.0)
1880 .................... return (1.0e+36);
1881 ....................
1882 .................... s = sin(x);
1883 .................... return(s/c);
1884 .................... }
1885 .................... #endif
1886 ....................
1887 .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290};
1888 .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290};
1889 ....................
1890 .................... float32 ASIN_COS(float32 x, unsigned int8 n)
1891 .................... {
1892 .................... float32 y, res, r, y2;
1893 .................... int1 s;
1894 .................... #ifdef _ERRNO
1895 .................... if(x <-1 || x > 1)
1896 .................... {
1897 .................... errno=EDOM;
1898 .................... }
1899 .................... #endif
1900 .................... s = 0;
1901 .................... y = x;
1902 ....................
1903 .................... if (x < 0)
1904 .................... {
1905 .................... s = 1;
1906 .................... y = -y;
1907 .................... }
1908 ....................
1909 .................... if (y > 0.5)
1910 .................... {
1911 .................... y = sqrt((1.0 - y)/2.0);
1912 .................... n += 2;
1913 .................... }
1914 ....................
1915 .................... y2=y*y;
1916 ....................
1917 .................... res = pas[0]*y2 + pas[1];
1918 .................... res = res*y2 + pas[2];
1919 ....................
1920 .................... r = qas[0]*y2 + qas[1];
1921 .................... r = r*y2 + qas[2];
1922 ....................
1923 .................... res = y*res/r;
1924 ....................
1925 .................... if (n & 2) // |x| > 0.5
1926 .................... res = PI_DIV_BY_TWO - 2.0*res;
1927 .................... if (s)
1928 .................... res = -res;
1929 .................... if (n & 1) // take arccos
1930 .................... res = PI_DIV_BY_TWO - res;
1931 ....................
1932 .................... return(res);
1933 .................... }
1934 ....................
1935 .................... //Overloaded functions for ASIN_COS() for PCD
1936 .................... // Overloaded function ASIN_COS() for data type - Float48
1937 .................... #if defined(__PCD__)
1938 .................... float48 ASIN_COS(float48 x, unsigned int8 n)
1939 .................... {
1940 .................... float48 y, res, r, y2;
1941 .................... int1 s;
1942 .................... #ifdef _ERRNO
1943 .................... if(x <-1 || x > 1)
1944 .................... {
1945 .................... errno=EDOM;
1946 .................... }
1947 .................... #endif
1948 .................... s = 0;
1949 .................... y = x;
1950 ....................
1951 .................... if (x < 0)
1952 .................... {
1953 .................... s = 1;
1954 .................... y = -y;
1955 .................... }
1956 ....................
1957 .................... if (y > 0.5)
1958 .................... {
1959 .................... y = sqrt((1.0 - y)/2.0);
1960 .................... n += 2;
1961 .................... }
1962 ....................
1963 .................... y2=y*y;
1964 ....................
1965 .................... res = pas[0]*y2 + pas[1];
1966 .................... res = res*y2 + pas[2];
1967 ....................
1968 .................... r = qas[0]*y2 + qas[1];
1969 .................... r = r*y2 + qas[2];
1970 ....................
1971 .................... res = y*res/r;
1972 ....................
1973 .................... if (n & 2) // |x| > 0.5
1974 .................... res = PI_DIV_BY_TWO - 2.0*res;
1975 .................... if (s)
1976 .................... res = -res;
1977 .................... if (n & 1) // take arccos
1978 .................... res = PI_DIV_BY_TWO - res;
1979 ....................
1980 .................... return(res);
1981 .................... }
1982 ....................
1983 .................... // Overloaded function ASIN_COS() for data type - Float64
1984 .................... float64 ASIN_COS(float64 x, unsigned int8 n)
1985 .................... {
1986 .................... float64 y, res, r, y2;
1987 .................... int1 s;
1988 .................... #ifdef _ERRNO
1989 .................... if(x <-1 || x > 1)
1990 .................... {
1991 .................... errno=EDOM;
1992 .................... }
1993 .................... #endif
1994 .................... s = 0;
1995 .................... y = x;
1996 ....................
1997 .................... if (x < 0)
1998 .................... {
1999 .................... s = 1;
2000 .................... y = -y;
2001 .................... }
2002 ....................
2003 .................... if (y > 0.5)
2004 .................... {
2005 .................... y = sqrt((1.0 - y)/2.0);
2006 .................... n += 2;
2007 .................... }
2008 ....................
2009 .................... y2=y*y;
2010 ....................
2011 .................... res = pas[0]*y2 + pas[1];
2012 .................... res = res*y2 + pas[2];
2013 ....................
2014 .................... r = qas[0]*y2 + qas[1];
2015 .................... r = r*y2 + qas[2];
2016 ....................
2017 .................... res = y*res/r;
2018 ....................
2019 .................... if (n & 2) // |x| > 0.5
2020 .................... res = PI_DIV_BY_TWO - 2.0*res;
2021 .................... if (s)
2022 .................... res = -res;
2023 .................... if (n & 1) // take arccos
2024 .................... res = PI_DIV_BY_TWO - res;
2025 ....................
2026 .................... return(res);
2027 .................... }
2028 .................... #endif
2029 ....................
2030 .................... ////////////////////////////////////////////////////////////////////////////
2031 .................... // float asin(float x)
2032 .................... ////////////////////////////////////////////////////////////////////////////
2033 .................... // Description : returns the arcsine value of the value x.
2034 .................... // Date : N/A
2035 .................... //
2036 .................... float32 asin(float32 x)
2037 .................... {
2038 .................... float32 r;
2039 ....................
2040 .................... r = ASIN_COS(x, 0);
2041 .................... return(r);
2042 .................... }
2043 .................... //Overloaded functions for asin() for PCD
2044 .................... // Overloaded function asin() for data type - Float48
2045 .................... #if defined(__PCD__)
2046 .................... float48 asin(float48 x)
2047 .................... {
2048 .................... float48 r;
2049 ....................
2050 .................... r = ASIN_COS(x, 0);
2051 .................... return(r);
2052 .................... }
2053 ....................
2054 .................... // Overloaded function asin() for data type - Float64
2055 .................... float64 asin(float64 x)
2056 .................... {
2057 .................... float64 r;
2058 ....................
2059 .................... r = ASIN_COS(x, 0);
2060 .................... return(r);
2061 .................... }
2062 .................... #endif
2063 ....................
2064 .................... ////////////////////////////////////////////////////////////////////////////
2065 .................... // float acos(float x)
2066 .................... ////////////////////////////////////////////////////////////////////////////
2067 .................... // Description : returns the arccosine value of the value x.
2068 .................... // Date : N/A
2069 .................... //
2070 .................... float32 acos(float32 x)
2071 .................... {
2072 .................... float32 r;
2073 ....................
2074 .................... r = ASIN_COS(x, 1);
2075 .................... return(r);
2076 .................... }
2077 .................... //Overloaded functions for acos() for PCD
2078 .................... // Overloaded function acos() for data type - Float48
2079 .................... #if defined(__PCD__)
2080 .................... float48 acos(float48 x)
2081 .................... {
2082 .................... float48 r;
2083 ....................
2084 .................... r = ASIN_COS(x, 1);
2085 .................... return(r);
2086 .................... }
2087 ....................
2088 .................... // Overloaded function acos() for data type - Float64
2089 .................... float64 acos(float64 x)
2090 .................... {
2091 .................... float64 r;
2092 ....................
2093 .................... r = ASIN_COS(x, 1);
2094 .................... return(r);
2095 .................... }
2096 .................... #endif
2097 ....................
2098 .................... 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};
2100 ....................
2101 .................... ////////////////////////////////////////////////////////////////////////////
2102 .................... // float atan(float x)
2103 .................... ////////////////////////////////////////////////////////////////////////////
2104 .................... // Description : returns the arctangent value of the value x.
2105 .................... // Date : N/A
2106 .................... //
2107 .................... float32 atan(float32 x)
2108 .................... {
2109 .................... float32 y, res, r;
2110 .................... int1 s, flag;
2111 ....................
2112 .................... s = 0;
2113 .................... flag = 0;
2114 .................... y = x;
2115 ....................
2116 .................... if (x < 0)
2117 .................... {
2118 .................... s = 1;
2119 .................... y = -y;
2120 .................... }
2121 ....................
2122 .................... if (y > 1.0)
2123 .................... {
2124 .................... y = 1.0/y;
2125 .................... flag = 1;
2126 .................... }
2127 ....................
2128 .................... res = pat[0]*y*y + pat[1];
2129 .................... res = res*y*y + pat[2];
2130 .................... res = res*y*y + pat[3];
2131 ....................
2132 .................... r = qat[0]*y*y + qat[1];
2133 .................... r = r*y*y + qat[2];
2134 .................... r = r*y*y + qat[3];
2135 ....................
2136 .................... res = y*res/r;
2137 ....................
2138 ....................
2139 .................... if (flag) // for |x| > 1
2140 .................... res = PI_DIV_BY_TWO - res;
2141 .................... if (s)
2142 .................... res = -res;
2143 ....................
2144 .................... return(res);
2145 .................... }
2146 .................... //Overloaded functions for atan() for PCD
2147 .................... // Overloaded function atan() for data type - Float48
2148 .................... #if defined(__PCD__)
2149 .................... float48 atan(float48 x)
2150 .................... {
2151 .................... float48 y, res, r;
2152 .................... int1 s, flag;
2153 ....................
2154 .................... s = 0;
2155 .................... flag = 0;
2156 .................... y = x;
2157 ....................
2158 .................... if (x < 0)
2159 .................... {
2160 .................... s = 1;
2161 .................... y = -y;
2162 .................... }
2163 ....................
2164 .................... if (y > 1.0)
2165 .................... {
2166 .................... y = 1.0/y;
2167 .................... flag = 1;
2168 .................... }
2169 ....................
2170 .................... res = pat[0]*y*y + pat[1];
2171 .................... res = res*y*y + pat[2];
2172 .................... res = res*y*y + pat[3];
2173 ....................
2174 .................... r = qat[0]*y*y + qat[1];
2175 .................... r = r*y*y + qat[2];
2176 .................... r = r*y*y + qat[3];
2177 ....................
2178 .................... res = y*res/r;
2179 ....................
2180 ....................
2181 .................... if (flag) // for |x| > 1
2182 .................... res = PI_DIV_BY_TWO - res;
2183 .................... if (s)
2184 .................... res = -res;
2185 ....................
2186 .................... return(res);
2187 .................... }
2188 ....................
2189 .................... // Overloaded function atan() for data type - Float64
2190 .................... float64 atan(float64 x)
2191 .................... {
2192 .................... float64 y, res, r;
2193 .................... int1 s, flag;
2194 ....................
2195 .................... s = 0;
2196 .................... flag = 0;
2197 .................... y = x;
2198 ....................
2199 .................... if (x < 0)
2200 .................... {
2201 .................... s = 1;
2202 .................... y = -y;
2203 .................... }
2204 ....................
2205 .................... if (y > 1.0)
2206 .................... {
2207 .................... y = 1.0/y;
2208 .................... flag = 1;
2209 .................... }
2210 ....................
2211 .................... res = pat[0]*y*y + pat[1];
2212 .................... res = res*y*y + pat[2];
2213 .................... res = res*y*y + pat[3];
2214 ....................
2215 .................... r = qat[0]*y*y + qat[1];
2216 .................... r = r*y*y + qat[2];
2217 .................... r = r*y*y + qat[3];
2218 ....................
2219 .................... res = y*res/r;
2220 ....................
2221 ....................
2222 .................... if (flag) // for |x| > 1
2223 .................... res = PI_DIV_BY_TWO - res;
2224 .................... if (s)
2225 .................... res = -res;
2226 ....................
2227 .................... return(res);
2228 .................... }
2229 .................... #endif
2230 ....................
2231 .................... /////////////////////////////////////////////////////////////////////////////
2232 .................... // float atan2(float y, float x)
2233 .................... /////////////////////////////////////////////////////////////////////////////
2234 .................... // 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
2236 .................... // Returns : returns the arc tangent of y/x.
2237 .................... // Date : N/A
2238 .................... //
2239 ....................
2240 .................... float32 atan2(float32 y,float32 x)
2241 .................... {
2242 .................... float32 z;
2243 .................... int1 sign;
2244 .................... unsigned int8 quad;
2245 .................... sign=0;
2246 .................... quad=0; //quadrant
2247 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2248 .................... if(y<0.0)
2249 .................... {
2250 .................... sign=1;
2251 .................... y=-y;
2252 .................... }
2253 .................... if(x<0.0)
2254 .................... {
2255 .................... x=-x;
2256 .................... }
2257 .................... if (x==0.0)
2258 .................... {
2259 .................... if(y==0.0)
2260 .................... {
2261 .................... #ifdef _ERRNO
2262 .................... {
2263 .................... errno=EDOM;
2264 .................... }
2265 .................... #endif
2266 .................... }
2267 .................... else
2268 .................... {
2269 .................... if(sign)
2270 .................... {
2271 .................... return (-(PI_DIV_BY_TWO));
2272 .................... }
2273 .................... else
2274 .................... {
2275 .................... return (PI_DIV_BY_TWO);
2276 .................... }
2277 .................... }
2278 .................... }
2279 .................... else
2280 .................... {
2281 .................... z=y/x;
2282 .................... switch(quad)
2283 .................... {
2284 .................... case 1:
2285 .................... {
2286 .................... return atan(z);
2287 .................... break;
2288 .................... }
2289 .................... case 2:
2290 .................... {
2291 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2292 .................... return (PI-atan(z));
2293 .................... break;
2294 .................... }
2295 .................... case 3:
2296 .................... {
2297 .................... return (atan(z)-PI);
2298 .................... break;
2299 .................... }
2300 .................... case 4:
2301 .................... {
2302 .................... return (-atan(z));
2303 .................... break;
2304 .................... }
2305 .................... }
2306 .................... }
2307 .................... }
2308 ....................
2309 .................... //Overloaded functions for atan2() for PCD
2310 .................... // Overloaded function atan2() for data type - Float48
2311 .................... #if defined(__PCD__)
2312 .................... float48 atan2(float48 y,float48 x)
2313 .................... {
2314 .................... float48 z;
2315 .................... int1 sign;
2316 .................... unsigned int8 quad;
2317 .................... sign=0;
2318 .................... quad=0; //quadrant
2319 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2320 .................... if(y<0.0)
2321 .................... {
2322 .................... sign=1;
2323 .................... y=-y;
2324 .................... }
2325 .................... if(x<0.0)
2326 .................... {
2327 .................... x=-x;
2328 .................... }
2329 .................... if (x==0.0)
2330 .................... {
2331 .................... if(y==0.0)
2332 .................... {
2333 .................... #ifdef _ERRNO
2334 .................... {
2335 .................... errno=EDOM;
2336 .................... }
2337 .................... #endif
2338 .................... }
2339 .................... else
2340 .................... {
2341 .................... if(sign)
2342 .................... {
2343 .................... return (-(PI_DIV_BY_TWO));
2344 .................... }
2345 .................... else
2346 .................... {
2347 .................... return (PI_DIV_BY_TWO);
2348 .................... }
2349 .................... }
2350 .................... }
2351 .................... else
2352 .................... {
2353 .................... z=y/x;
2354 .................... switch(quad)
2355 .................... {
2356 .................... case 1:
2357 .................... {
2358 .................... return atan(z);
2359 .................... break;
2360 .................... }
2361 .................... case 2:
2362 .................... {
2363 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2364 .................... return (PI-atan(z));
2365 .................... break;
2366 .................... }
2367 .................... case 3:
2368 .................... {
2369 .................... return (atan(z)-PI);
2370 .................... break;
2371 .................... }
2372 .................... case 4:
2373 .................... {
2374 .................... return (-atan(z));
2375 .................... break;
2376 .................... }
2377 .................... }
2378 .................... }
2379 .................... }
2380 ....................
2381 .................... // Overloaded function atan2() for data type - Float64
2382 .................... float64 atan2(float64 y,float64 x)
2383 .................... {
2384 .................... float64 z;
2385 .................... int1 sign;
2386 .................... unsigned int8 quad;
2387 .................... sign=0;
2388 .................... quad=0; //quadrant
2389 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2390 .................... if(y<0.0)
2391 .................... {
2392 .................... sign=1;
2393 .................... y=-y;
2394 .................... }
2395 .................... if(x<0.0)
2396 .................... {
2397 .................... x=-x;
2398 .................... }
2399 .................... if (x==0.0)
2400 .................... {
2401 .................... if(y==0.0)
2402 .................... {
2403 .................... #ifdef _ERRNO
2404 .................... {
2405 .................... errno=EDOM;
2406 .................... }
2407 .................... #endif
2408 .................... }
2409 .................... else
2410 .................... {
2411 .................... if(sign)
2412 .................... {
2413 .................... return (-(PI_DIV_BY_TWO));
2414 .................... }
2415 .................... else
2416 .................... {
2417 .................... return (PI_DIV_BY_TWO);
2418 .................... }
2419 .................... }
2420 .................... }
2421 .................... else
2422 .................... {
2423 .................... z=y/x;
2424 .................... switch(quad)
2425 .................... {
2426 .................... case 1:
2427 .................... {
2428 .................... return atan(z);
2429 .................... break;
2430 .................... }
2431 .................... case 2:
2432 .................... {
2433 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2434 .................... return (PI-atan(z));
2435 .................... break;
2436 .................... }
2437 .................... case 3:
2438 .................... {
2439 .................... return (atan(z)-PI);
2440 .................... break;
2441 .................... }
2442 .................... case 4:
2443 .................... {
2444 .................... return (-atan(z));
2445 .................... break;
2446 .................... }
2447 .................... }
2448 .................... }
2449 .................... }
2450 .................... #endif
2451 ....................
2452 .................... //////////////////// Hyperbolic functions ////////////////////
2453 ....................
2454 .................... ////////////////////////////////////////////////////////////////////////////
2455 .................... // float cosh(float x)
2456 .................... ////////////////////////////////////////////////////////////////////////////
2457 .................... // Description : Computes the hyperbolic cosine value of x
2458 .................... // Returns : returns the hyperbolic cosine value of x
2459 .................... // Date : N/A
2460 .................... //
2461 ....................
2462 .................... float32 cosh(float32 x)
2463 .................... {
2464 .................... return ((exp(x)+exp(-x))/2);
2465 .................... }
2466 .................... //Overloaded functions for cosh() for PCD
2467 .................... // Overloaded function cosh() for data type - Float48
2468 .................... #if defined(__PCD__)
2469 .................... float48 cosh(float48 x)
2470 .................... {
2471 .................... return ((exp(x)+exp(-x))/2);
2472 .................... }
2473 ....................
2474 .................... // Overloaded function cosh() for data type - Float64
2475 .................... float64 cosh(float64 x)
2476 .................... {
2477 .................... return ((exp(x)+exp(-x))/2);
2478 .................... }
2479 .................... #endif
2480 ....................
2481 .................... ////////////////////////////////////////////////////////////////////////////
2482 .................... // float sinh(float x)
2483 .................... ////////////////////////////////////////////////////////////////////////////
2484 .................... // Description : Computes the hyperbolic sine value of x
2485 .................... // Returns : returns the hyperbolic sine value of x
2486 .................... // Date : N/A
2487 .................... //
2488 ....................
2489 .................... float32 sinh(float32 x)
2490 .................... {
2491 ....................
2492 .................... return ((exp(x) - exp(-x))/2);
2493 .................... }
2494 .................... //Overloaded functions for sinh() for PCD
2495 .................... // Overloaded function sinh() for data type - Float48
2496 .................... #if defined(__PCD__)
2497 .................... float48 sinh(float48 x)
2498 .................... {
2499 ....................
2500 .................... return ((exp(x) - exp(-x))/2);
2501 .................... }
2502 ....................
2503 .................... // Overloaded function sinh() for data type - Float48
2504 .................... float64 sinh(float64 x)
2505 .................... {
2506 ....................
2507 .................... return ((exp(x) - exp(-x))/2);
2508 .................... }
2509 .................... #endif
2510 ....................
2511 .................... ////////////////////////////////////////////////////////////////////////////
2512 .................... // float tanh(float x)
2513 .................... ////////////////////////////////////////////////////////////////////////////
2514 .................... // Description : Computes the hyperbolic tangent value of x
2515 .................... // Returns : returns the hyperbolic tangent value of x
2516 .................... // Date : N/A
2517 .................... //
2518 ....................
2519 .................... float32 tanh(float32 x)
2520 .................... {
2521 .................... return(sinh(x)/cosh(x));
2522 .................... }
2523 .................... //Overloaded functions for tanh() for PCD
2524 .................... // Overloaded function tanh() for data type - Float48
2525 .................... #if defined(__PCD__)
2526 .................... float48 tanh(float48 x)
2527 .................... {
2528 .................... return(sinh(x)/cosh(x));
2529 .................... }
2530 ....................
2531 .................... // Overloaded function tanh() for data type - Float64
2532 .................... float64 tanh(float64 x)
2533 .................... {
2534 .................... return(sinh(x)/cosh(x));
2535 .................... }
2536 .................... #endif
2537 ....................
2538 .................... ////////////////////////////////////////////////////////////////////////////
2539 .................... // float frexp(float x, signed int *exp)
2540 .................... ////////////////////////////////////////////////////////////////////////////
2541 .................... // 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.
2543 .................... // 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,
2545 .................... // both parts of the result are zero.
2546 .................... // Date : N/A
2547 .................... //
2548 ....................
2549 .................... #define LOG2 .30102999566398119521
2550 .................... float32 frexp(float32 x, signed int8 *exp)
2551 .................... {
2552 .................... float32 res;
2553 .................... int1 sign = 0;
2554 .................... if(x == 0.0)
2555 .................... {
2556 .................... *exp=0;
2557 .................... return (0.0);
2558 .................... }
2559 .................... if(x < 0.0)
2560 .................... {
2561 .................... x=-x;
2562 .................... sign=1;
2563 .................... }
2564 .................... if (x > 1.0)
2565 .................... {
2566 .................... *exp=(ceil(log10(x)/LOG2));
2567 .................... res=x/(pow(2, *exp));
2568 .................... if (res == 1)
2569 .................... {
2570 .................... *exp=*exp+1;
2571 .................... res=.5;
2572 .................... }
2573 .................... }
2574 .................... else
2575 .................... {
2576 .................... if(x < 0.5)
2577 .................... {
2578 .................... *exp=-1;
2579 .................... res=x*2;
2580 .................... }
2581 .................... else
2582 .................... {
2583 .................... *exp=0;
2584 .................... res=x;
2585 .................... }
2586 .................... }
2587 .................... if(sign)
2588 .................... {
2589 .................... res=-res;
2590 .................... }
2591 .................... return res;
2592 .................... }
2593 ....................
2594 .................... //Overloaded functions for frexp() for PCD
2595 .................... // Overloaded function frexp() for data type - Float48
2596 .................... #if defined(__PCD__)
2597 .................... float48 frexp(float48 x, signed int8 *exp)
2598 .................... {
2599 .................... float48 res;
2600 .................... int1 sign = 0;
2601 .................... if(x == 0.0)
2602 .................... {
2603 .................... *exp=0;
2604 .................... return (0.0);
2605 .................... }
2606 .................... if(x < 0.0)
2607 .................... {
2608 .................... x=-x;
2609 .................... sign=1;
2610 .................... }
2611 .................... if (x > 1.0)
2612 .................... {
2613 .................... *exp=(ceil(log10(x)/LOG2));
2614 .................... res=x/(pow(2, *exp));
2615 .................... if (res == 1)
2616 .................... {
2617 .................... *exp=*exp+1;
2618 .................... res=.5;
2619 .................... }
2620 .................... }
2621 .................... else
2622 .................... {
2623 .................... if(x < 0.5)
2624 .................... {
2625 .................... *exp=-1;
2626 .................... res=x*2;
2627 .................... }
2628 .................... else
2629 .................... {
2630 .................... *exp=0;
2631 .................... res=x;
2632 .................... }
2633 .................... }
2634 .................... if(sign)
2635 .................... {
2636 .................... res=-res;
2637 .................... }
2638 .................... return res;
2639 .................... }
2640 ....................
2641 .................... // Overloaded function frexp() for data type - Float64
2642 .................... float64 frexp(float64 x, signed int8 *exp)
2643 .................... {
2644 .................... float64 res;
2645 .................... int1 sign = 0;
2646 .................... if(x == 0.0)
2647 .................... {
2648 .................... *exp=0;
2649 .................... return (0.0);
2650 .................... }
2651 .................... if(x < 0.0)
2652 .................... {
2653 .................... x=-x;
2654 .................... sign=1;
2655 .................... }
2656 .................... if (x > 1.0)
2657 .................... {
2658 .................... *exp=(ceil(log10(x)/LOG2));
2659 .................... res=x/(pow(2, *exp));
2660 .................... if (res == 1)
2661 .................... {
2662 .................... *exp=*exp+1;
2663 .................... res=.5;
2664 .................... }
2665 .................... }
2666 .................... else
2667 .................... {
2668 .................... if(x < 0.5)
2669 .................... {
2670 .................... *exp=-1;
2671 .................... res=x*2;
2672 .................... }
2673 .................... else
2674 .................... {
2675 .................... *exp=0;
2676 .................... res=x;
2677 .................... }
2678 .................... }
2679 .................... if(sign)
2680 .................... {
2681 .................... res=-res;
2682 .................... }
2683 .................... return res;
2684 .................... }
2685 .................... #endif
2686 ....................
2687 .................... //////////////////////////////////////////////////////////////////////////////
2688 .................... // float ldexp(float x, signed int *exp)
2689 .................... //////////////////////////////////////////////////////////////////////////////
2690 .................... // 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.
2692 .................... // Date : N/A
2693 .................... //
2694 ....................
2695 .................... float32 ldexp(float32 value, signed int8 exp)
2696 .................... {
2697 .................... return (value * pow(2,exp));
2698 .................... }
2699 .................... //Overloaded functions for ldexp() for PCD
2700 .................... // Overloaded function ldexp() for data type - Float48
2701 ....................
2702 .................... #if defined(__PCD__)
2703 .................... float48 ldexp(float48 value, signed int8 exp)
2704 .................... {
2705 .................... return (value * pow(2,exp));
2706 .................... }
2707 .................... // Overloaded function ldexp() for data type - Float64
2708 .................... float64 ldexp(float64 value, signed int8 exp)
2709 .................... {
2710 .................... return (value * pow(2,exp));
2711 .................... }
2712 .................... #endif
2713 ....................
2714 .................... #endif
2715 ....................
2716 ....................
2926 jacho 2717 .................... void main()
2718 .................... {
2719 *
2998 kaklik 2720 0266: CLRF 04
2721 0267: BCF 03.7
2722 0268: MOVLW 1F
2723 0269: ANDWF 03,F
2724 026A: MOVLW 71
2725 026B: BSF 03.5
2726 026C: MOVWF 0F
2727 026D: MOVF 0F,W
2728 026E: BCF 03.5
2729 026F: BCF 20.7
2730 0270: MOVF 20,W
2731 0271: BSF 03.5
2732 0272: MOVWF 07
2733 0273: BCF 03.5
2734 0274: BSF 07.7
2735 0275: BSF 03.5
2736 0276: BSF 03.6
2737 0277: MOVF 09,W
2738 0278: ANDLW C0
2739 0279: MOVWF 09
2740 027A: BCF 03.6
2741 027B: BCF 1F.4
2742 027C: BCF 1F.5
2743 027D: MOVLW 00
2744 027E: BSF 03.6
2745 027F: MOVWF 08
2746 0280: BCF 03.5
2747 0281: CLRF 07
2748 0282: CLRF 08
2749 0283: CLRF 09
2985 kaklik 2750 *
2998 kaklik 2751 028D: CLRF 34
2752 028E: CLRF 33
2988 kaklik 2753 .................... float last,b,anemo_speed;
2985 kaklik 2754 .................... unsigned int16 anemo_round=0;
2755 .................... unsigned int16 i;
2756 ....................
2757 .................... //signed int16 X,Y,Z;
2984 kaklik 2758 .................... setup_adc_ports(NO_ANALOGS|VSS_VDD);
2998 kaklik 2759 028F: BSF 03.5
2760 0290: BSF 03.6
2761 0291: MOVF 09,W
2762 0292: ANDLW C0
2763 0293: MOVWF 09
2764 0294: BCF 03.6
2765 0295: BCF 1F.4
2766 0296: BCF 1F.5
2767 0297: MOVLW 00
2768 0298: BSF 03.6
2769 0299: MOVWF 08
2984 kaklik 2770 .................... setup_adc(ADC_CLOCK_DIV_2);
2998 kaklik 2771 029A: BCF 03.5
2772 029B: BCF 03.6
2773 029C: BCF 1F.6
2774 029D: BCF 1F.7
2775 029E: BSF 03.5
2776 029F: BCF 1F.7
2777 02A0: BCF 03.5
2778 02A1: BSF 1F.0
2984 kaklik 2779 .................... setup_spi(SPI_SS_DISABLED);
2998 kaklik 2780 02A2: BCF 14.5
2781 02A3: BCF 20.5
2782 02A4: MOVF 20,W
2783 02A5: BSF 03.5
2784 02A6: MOVWF 07
2785 02A7: BCF 03.5
2786 02A8: BSF 20.4
2787 02A9: MOVF 20,W
2988 kaklik 2788 02AA: BSF 03.5
2998 kaklik 2789 02AB: MOVWF 07
2988 kaklik 2790 02AC: BCF 03.5
2998 kaklik 2791 02AD: BCF 20.3
2988 kaklik 2792 02AE: MOVF 20,W
2793 02AF: BSF 03.5
2794 02B0: MOVWF 07
2998 kaklik 2795 02B1: MOVLW 01
2796 02B2: BCF 03.5
2797 02B3: MOVWF 14
2798 02B4: MOVLW 00
2799 02B5: BSF 03.5
2800 02B6: MOVWF 14
2801 .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
2802 02B7: MOVF 01,W
2803 02B8: ANDLW C7
2804 02B9: IORLW 08
2805 02BA: MOVWF 01
2806 .................... setup_timer_1(T1_DISABLED);
2807 02BB: BCF 03.5
2808 02BC: CLRF 10
2809 .................... setup_timer_2(T2_DISABLED,0,1);
2810 02BD: MOVLW 00
2811 02BE: MOVWF 78
2812 02BF: MOVWF 12
2813 02C0: MOVLW 00
2814 02C1: BSF 03.5
2815 02C2: MOVWF 12
2816 .................... setup_ccp1(CCP_OFF);
2817 02C3: BCF 03.5
2818 02C4: BSF 20.2
2819 02C5: MOVF 20,W
2820 02C6: BSF 03.5
2821 02C7: MOVWF 07
2822 02C8: BCF 03.5
2823 02C9: CLRF 17
2824 02CA: BSF 03.5
2825 02CB: CLRF 1B
2826 02CC: CLRF 1C
2827 02CD: MOVLW 01
2828 02CE: MOVWF 1D
2984 kaklik 2829 .................... setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard
2998 kaklik 2830 02CF: BCF 03.5
2831 02D0: BSF 03.6
2832 02D1: CLRF 07
2833 02D2: CLRF 08
2834 02D3: CLRF 09
2926 jacho 2835 ....................
2998 kaklik 2836 .................... printf("Magnetometr: \r\n",);
2837 02D4: MOVLW 04
2838 02D5: MOVWF 0D
2839 02D6: MOVLW 00
2840 02D7: MOVWF 0F
2841 02D8: BCF 03.6
2842 02D9: CALL 043
2843 .................... printf("(c)mlab JACHO 2013: \r\n",);
2844 02DA: MOVLW 0D
2845 02DB: BSF 03.6
2846 02DC: MOVWF 0D
2847 02DD: MOVLW 00
2848 02DE: MOVWF 0F
2849 02DF: BCF 03.6
2850 02E0: CALL 043
2851 .................... printf("X, Y, Z \r\n",);
2852 02E1: MOVLW 19
2853 02E2: BSF 03.6
2854 02E3: MOVWF 0D
2855 02E4: MOVLW 00
2856 02E5: MOVWF 0F
2857 02E6: BCF 03.6
2858 02E7: CALL 043
2859 ....................
2860 ....................
2985 kaklik 2861 .................... // Init the HMC5883L. Set Mode register for
2862 .................... // continuous measurements.
2863 .................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18); // no average, maximal update range
2998 kaklik 2864 02E8: CLRF 37
2865 02E9: MOVLW 18
2866 02EA: MOVWF 38
2867 02EB: CALL 0D3
2985 kaklik 2868 .................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0xE0); // maximal range
2998 kaklik 2869 02EC: MOVLW 01
2870 02ED: MOVWF 37
2871 02EE: MOVLW E0
2872 02EF: MOVWF 38
2873 02F0: CALL 0D3
2985 kaklik 2874 .................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00);
2998 kaklik 2875 02F1: MOVLW 02
2876 02F2: MOVWF 37
2877 02F3: CLRF 38
2878 02F4: CALL 0D3
2926 jacho 2879 ....................
2985 kaklik 2880 .................... // Continuously read and display the x,y,z results.
2881 .................... // Wait at least 67 ms between reads, re the HMC5883L data sheet.
2882 ....................
2883 ....................
2884 .................... while(TRUE)
2885 .................... {
2886 ....................
2998 kaklik 2887 .................... hmc5883l_read_data();
2888 02F5: GOTO 151
2889 .................... printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z);
2890 02F6: MOVLW 00
2891 02F7: MOVWF 04
2892 02F8: MOVF 22,W
2893 02F9: MOVWF 38
2894 02FA: MOVF 21,W
2895 02FB: MOVWF 37
2896 02FC: CALL 1D0
2897 02FD: MOVLW 20
2898 02FE: MOVWF 40
2899 02FF: CALL 01F
2900 0300: MOVLW 00
2901 0301: MOVWF 04
2902 0302: MOVF 24,W
2903 0303: MOVWF 38
2904 0304: MOVF 23,W
2905 0305: MOVWF 37
2906 0306: CALL 1D0
2907 0307: MOVLW 20
2908 0308: MOVWF 40
2909 0309: CALL 01F
2910 030A: MOVLW 00
2911 030B: MOVWF 04
2912 030C: MOVF 26,W
2913 030D: MOVWF 38
2914 030E: MOVF 25,W
2915 030F: MOVWF 37
2916 0310: CALL 1D0
2917 0311: MOVLW 20
2918 0312: MOVWF 40
2919 0313: CALL 01F
2920 0314: MOVLW 0A
2921 0315: MOVWF 40
2922 0316: CALL 01F
2923 0317: MOVLW 0D
2924 0318: MOVWF 40
2925 0319: CALL 01F
2926 .................... delay_ms(100);
2927 031A: MOVLW 64
2928 031B: MOVWF 37
2929 031C: GOTO 250
2985 kaklik 2930 .................... }
2998 kaklik 2931 031D: GOTO 2F5
2985 kaklik 2932 ....................
2926 jacho 2933 .................... }
2985 kaklik 2934 ....................
2935 ....................
2936 ....................
2998 kaklik 2937 031E: SLEEP
2926 jacho 2938  
2939 Configuration Fuses:
2940 Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG
2941 Word 2: 3FFF NOWRT BORV40