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