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