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