Rev 2988 Rev 2998
1 CCS PCM C Compiler, Version 4.106, 47914 15-V-13 20:34 1 CCS PCM C Compiler, Version 4.106, 47914 17-V-13 12:14
2   2  
3 Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst 3 Filename: Z:\home\kaklik\svnMLAB\Modules\Sensors\MAG01A\SW\PIC16F887\main.lst
4   4  
5 ROM used: 775 words (9%) 5 ROM used: 799 words (10%)
6 Largest free fragment is 2048 6 Largest free fragment is 2048
7 RAM used: 28 (8%) at main() level 7 RAM used: 28 (8%) at main() level
8 38 (10%) worst case 8 38 (10%) worst case
9 Stack: 2 locations 9 Stack: 2 locations
10   10  
11 * 11 *
12 0000: MOVLW 02 12 0000: MOVLW 02
13 0001: MOVWF 0A 13 0001: MOVWF 0A
14 0002: GOTO 24F 14 0002: GOTO 266
15 0003: NOP 15 0003: NOP
16 .................... #include "main.h" 16 .................... #include "main.h"
17 .................... #include <16F887.h> 17 .................... #include <16F887.h>
18 .................... //////// Standard Header file for the PIC16F887 device //////////////// 18 .................... //////// Standard Header file for the PIC16F887 device ////////////////
19 .................... #device PIC16F887 19 .................... #device PIC16F887
20 .................... #list 20 .................... #list
21 .................... 21 ....................
22 .................... #device adc=8 22 .................... #device adc=8
23 .................... 23 ....................
24 .................... #FUSES NOWDT //No Watch Dog Timer 24 .................... #FUSES NOWDT //No Watch Dog Timer
25 .................... #FUSES INTRC //Internal RC Osc 25 .................... #FUSES INTRC //Internal RC Osc
26 .................... #FUSES NOPUT //No Power Up Timer 26 .................... #FUSES NOPUT //No Power Up Timer
27 .................... #FUSES MCLR //Master Clear pin enabled 27 .................... #FUSES MCLR //Master Clear pin enabled
28 .................... #FUSES NOPROTECT //Code not protected from reading 28 .................... #FUSES NOPROTECT //Code not protected from reading
29 .................... #FUSES NOCPD //No EE protection 29 .................... #FUSES NOCPD //No EE protection
30 .................... #FUSES NOBROWNOUT //No brownout reset 30 .................... #FUSES NOBROWNOUT //No brownout reset
31 .................... #FUSES IESO //Internal External Switch Over mode enabled 31 .................... #FUSES IESO //Internal External Switch Over mode enabled
32 .................... #FUSES FCMEN //Fail-safe clock monitor enabled 32 .................... #FUSES FCMEN //Fail-safe clock monitor enabled
33 .................... #FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O 33 .................... #FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
34 .................... #FUSES NODEBUG //No Debug mode for ICD 34 .................... #FUSES NODEBUG //No Debug mode for ICD
35 .................... #FUSES NOWRT //Program memory not write protected 35 .................... #FUSES NOWRT //Program memory not write protected
36 .................... #FUSES BORV40 //Brownout reset at 4.0V 36 .................... #FUSES BORV40 //Brownout reset at 4.0V
37 .................... 37 ....................
38 .................... #use delay(clock=8000000) 38 .................... #use delay(clock=8000000)
39 .................... #use i2c(master, sda=PIN_C4, scl=PIN_C3) -  
40 * 39 *
-   40 0250: MOVLW 37
-   41 0251: MOVWF 04
-   42 0252: BCF 03.7
-   43 0253: MOVF 00,W
-   44 0254: BTFSC 03.2
-   45 0255: GOTO 263
-   46 0256: MOVLW 02
-   47 0257: MOVWF 78
-   48 0258: CLRF 77
-   49 0259: DECFSZ 77,F
-   50 025A: GOTO 259
-   51 025B: DECFSZ 78,F
-   52 025C: GOTO 258
-   53 025D: MOVLW 97
-   54 025E: MOVWF 77
-   55 025F: DECFSZ 77,F
-   56 0260: GOTO 25F
-   57 0261: DECFSZ 00,F
-   58 0262: GOTO 256
-   59 0263: BCF 0A.3
-   60 0264: BCF 0A.4
-   61 0265: GOTO 31D (RETURN)
-   62 .................... #use i2c(Master,Slow,sda=PIN_C4,scl=PIN_C3)
-   63 *
41 001F: MOVLW 08 64 0089: MOVLW 08
42 0020: MOVWF 78 65 008A: MOVWF 78
43 0021: NOP 66 008B: NOP
44 0022: BCF 07.3 67 008C: BCF 07.3
45 0023: BCF 20.3 68 008D: BCF 20.3
46 0024: MOVF 20,W 69 008E: MOVF 20,W
47 0025: BSF 03.5 70 008F: BSF 03.5
48 0026: MOVWF 07 71 0090: MOVWF 07
49 0027: NOP 72 0091: NOP
50 0028: BCF 03.5 73 0092: BCF 03.5
51 0029: RLF 3D,F 74 0093: RLF 3D,F
52 002A: BCF 07.4 75 0094: BCF 07.4
53 002B: BTFSS 03.0 76 0095: BTFSS 03.0
54 002C: GOTO 033 77 0096: GOTO 09D
55 002D: BSF 20.4 78 0097: BSF 20.4
56 002E: MOVF 20,W 79 0098: MOVF 20,W
57 002F: BSF 03.5 80 0099: BSF 03.5
58 0030: MOVWF 07 81 009A: MOVWF 07
59 0031: GOTO 037 82 009B: GOTO 0A1
60 0032: BCF 03.5 83 009C: BCF 03.5
61 0033: BCF 20.4 84 009D: BCF 20.4
62 0034: MOVF 20,W 85 009E: MOVF 20,W
63 0035: BSF 03.5 86 009F: BSF 03.5
64 0036: MOVWF 07 87 00A0: MOVWF 07
65 0037: NOP 88 00A1: NOP
66 0038: BCF 03.5 89 00A2: BCF 03.5
67 0039: BSF 20.3 90 00A3: BSF 20.3
68 003A: MOVF 20,W 91 00A4: MOVF 20,W
69 003B: BSF 03.5 92 00A5: BSF 03.5
70 003C: MOVWF 07 93 00A6: MOVWF 07
71 003D: BCF 03.5 94 00A7: BCF 03.5
72 003E: BTFSS 07.3 95 00A8: BTFSS 07.3
73 003F: GOTO 03E 96 00A9: GOTO 0A8
74 0040: DECFSZ 78,F 97 00AA: DECFSZ 78,F
75 0041: GOTO 021 98 00AB: GOTO 08B
76 0042: NOP 99 00AC: NOP
77 0043: BCF 07.3 100 00AD: BCF 07.3
78 0044: BCF 20.3 101 00AE: BCF 20.3
79 0045: MOVF 20,W 102 00AF: MOVF 20,W
80 0046: BSF 03.5 103 00B0: BSF 03.5
81 0047: MOVWF 07 104 00B1: MOVWF 07
82 0048: NOP 105 00B2: NOP
83 0049: BCF 03.5 106 00B3: BCF 03.5
84 004A: BSF 20.4 107 00B4: BSF 20.4
85 004B: MOVF 20,W 108 00B5: MOVF 20,W
86 004C: BSF 03.5 109 00B6: BSF 03.5
87 004D: MOVWF 07 110 00B7: MOVWF 07
88 004E: NOP 111 00B8: NOP
89 004F: NOP 112 00B9: NOP
90 0050: BCF 03.5 113 00BA: BCF 03.5
91 0051: BSF 20.3 114 00BB: BSF 20.3
92 0052: MOVF 20,W 115 00BC: MOVF 20,W
93 0053: BSF 03.5 116 00BD: BSF 03.5
94 0054: MOVWF 07 117 00BE: MOVWF 07
95 0055: BCF 03.5 118 00BF: BCF 03.5
96 0056: BTFSS 07.3 119 00C0: BTFSS 07.3
97 0057: GOTO 056 120 00C1: GOTO 0C0
98 0058: CLRF 78 121 00C2: CLRF 78
99 0059: NOP 122 00C3: NOP
100 005A: BTFSC 07.4 123 00C4: BTFSC 07.4
101 005B: BSF 78.0 124 00C5: BSF 78.0
102 005C: BCF 07.3 125 00C6: BCF 07.3
103 005D: BCF 20.3 126 00C7: BCF 20.3
104 005E: MOVF 20,W 127 00C8: MOVF 20,W
105 005F: BSF 03.5 128 00C9: BSF 03.5
106 0060: MOVWF 07 129 00CA: MOVWF 07
107 0061: BCF 03.5 130 00CB: BCF 03.5
108 0062: BCF 07.4 131 00CC: BCF 07.4
109 0063: BCF 20.4 132 00CD: BCF 20.4
110 0064: MOVF 20,W 133 00CE: MOVF 20,W
111 0065: BSF 03.5 134 00CF: BSF 03.5
112 0066: MOVWF 07 135 00D0: MOVWF 07
113 0067: BCF 03.5 136 00D1: BCF 03.5
114 0068: RETURN 137 00D2: RETURN
115 * 138 *
116 010B: MOVLW 08 139 010C: MOVLW 08
117 010C: MOVWF 3E 140 010D: MOVWF 3E
118 010D: MOVF 77,W 141 010E: MOVF 77,W
119 010E: MOVWF 3F 142 010F: MOVWF 3F
120 010F: BSF 20.4 143 0110: BSF 20.4
121 0110: MOVF 20,W 144 0111: MOVF 20,W
122 0111: BSF 03.5 145 0112: BSF 03.5
123 0112: MOVWF 07 146 0113: MOVWF 07
124 0113: NOP 147 0114: NOP
125 0114: BCF 03.5 148 0115: BCF 03.5
126 0115: BSF 20.3 149 0116: BSF 20.3
127 0116: MOVF 20,W 150 0117: MOVF 20,W
128 0117: BSF 03.5 151 0118: BSF 03.5
129 0118: MOVWF 07 152 0119: MOVWF 07
130 0119: BCF 03.5 153 011A: BCF 03.5
131 011A: BTFSS 07.3 154 011B: BTFSS 07.3
132 011B: GOTO 11A 155 011C: GOTO 11B
133 011C: BTFSC 07.4 156 011D: BTFSC 07.4
134 011D: BSF 03.0 157 011E: BSF 03.0
135 011E: BTFSS 07.4 158 011F: BTFSS 07.4
136 011F: BCF 03.0 159 0120: BCF 03.0
137 0120: RLF 78,F 160 0121: RLF 78,F
138 0121: NOP 161 0122: NOP
139 0122: BCF 20.3 162 0123: BCF 20.3
140 0123: MOVF 20,W 163 0124: MOVF 20,W
141 0124: BSF 03.5 164 0125: BSF 03.5
142 0125: MOVWF 07 165 0126: MOVWF 07
143 0126: BCF 03.5 166 0127: BCF 03.5
144 0127: BCF 07.3 167 0128: BCF 07.3
145 0128: DECFSZ 3E,F 168 0129: DECFSZ 3E,F
146 0129: GOTO 10F 169 012A: GOTO 110
147 012A: BSF 20.4 170 012B: BSF 20.4
148 012B: MOVF 20,W 171 012C: MOVF 20,W
149 012C: BSF 03.5 172 012D: BSF 03.5
150 012D: MOVWF 07 173 012E: MOVWF 07
151 012E: NOP 174 012F: NOP
152 012F: BCF 03.5 175 0130: BCF 03.5
153 0130: BCF 07.4 176 0131: BCF 07.4
154 0131: MOVF 3F,W 177 0132: MOVF 3F,W
155 0132: BTFSC 03.2 178 0133: BTFSC 03.2
156 0133: GOTO 139 179 0134: GOTO 13A
157 0134: BCF 20.4 180 0135: BCF 20.4
158 0135: MOVF 20,W 181 0136: MOVF 20,W
159 0136: BSF 03.5 182 0137: BSF 03.5
160 0137: MOVWF 07 183 0138: MOVWF 07
161 0138: BCF 03.5 184 0139: BCF 03.5
162 0139: NOP 185 013A: NOP
163 013A: BSF 20.3 186 013B: BSF 20.3
164 013B: MOVF 20,W 187 013C: MOVF 20,W
165 013C: BSF 03.5 188 013D: BSF 03.5
166 013D: MOVWF 07 189 013E: MOVWF 07
167 013E: BCF 03.5 190 013F: BCF 03.5
168 013F: BTFSS 07.3 191 0140: BTFSS 07.3
169 0140: GOTO 13F 192 0141: GOTO 140
170 0141: NOP 193 0142: NOP
171 0142: BCF 07.3 194 0143: BCF 07.3
172 0143: BCF 20.3 195 0144: BCF 20.3
173 0144: MOVF 20,W 196 0145: MOVF 20,W
174 0145: BSF 03.5 197 0146: BSF 03.5
175 0146: MOVWF 07 198 0147: MOVWF 07
176 0147: NOP 199 0148: NOP
177 0148: BCF 03.5 200 0149: BCF 03.5
178 0149: BCF 07.4 201 014A: BCF 07.4
179 014A: BCF 20.4 202 014B: BCF 20.4
180 014B: MOVF 20,W 203 014C: MOVF 20,W
181 014C: BSF 03.5 204 014D: BSF 03.5
182 014D: MOVWF 07 205 014E: MOVWF 07
183 014E: BCF 03.5 206 014F: BCF 03.5
184 014F: RETURN 207 0150: RETURN
185 .................... #use rs232(baud=115200,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD 208 .................... #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD
186 * 209 *
187 00A2: BCF 20.7 210 001F: BCF 20.7
188 00A3: MOVF 20,W 211 0020: MOVF 20,W
189 00A4: BSF 03.5 212 0021: BSF 03.5
190 00A5: MOVWF 07 213 0022: MOVWF 07
191 00A6: BCF 03.5 214 0023: BCF 03.5
192 00A7: BCF 07.7 215 0024: BCF 07.7
193 00A8: MOVLW 08 216 0025: MOVLW 08
194 00A9: MOVWF 78 217 0026: MOVWF 78
195 00AA: GOTO 0AB 218 0027: GOTO 028
196 00AB: NOP 219 0028: NOP
197 00AC: BSF 78.7 220 0029: BSF 78.7
198 00AD: GOTO 0C0 221 002A: GOTO 039
199 00AE: BCF 78.7 222 002B: BCF 78.7
200 00AF: MOVF 40,W -  
201 00B0: MOVWF 79 -  
202 00B1: RRF 79,F 223 002C: RRF 40,F
203 00B2: BTFSC 03.0 224 002D: BTFSC 03.0
204 00B3: BSF 07.7 225 002E: BSF 07.7
205 00B4: BTFSS 03.0 226 002F: BTFSS 03.0
206 00B5: BCF 07.7 227 0030: BCF 07.7
207 00B6: BSF 78.6 228 0031: BSF 78.6
208 00B7: GOTO 0C0 229 0032: GOTO 039
209 00B8: BCF 78.6 230 0033: BCF 78.6
210 00B9: DECFSZ 78,F 231 0034: DECFSZ 78,F
211 00BA: GOTO 0B1 232 0035: GOTO 02C
212 00BB: MOVF 79,W 233 0036: GOTO 037
-   234 0037: NOP
-   235 0038: BSF 07.7
213 00BC: MOVWF 40 236 0039: MOVLW 3F
214 00BD: GOTO 0BE 237 003A: MOVWF 04
215 00BE: NOP 238 003B: DECFSZ 04,F
216 00BF: BSF 07.7 239 003C: GOTO 03B
-   240 003D: NOP
217 00C0: BTFSC 78.7 241 003E: BTFSC 78.7
218 00C1: GOTO 0AE 242 003F: GOTO 02B
219 00C2: BTFSC 78.6 243 0040: BTFSC 78.6
220 00C3: GOTO 0B8 244 0041: GOTO 033
221 00C4: RETURN 245 0042: RETURN
222 .................... 246 ....................
223 .................... 247 ....................
224 .................... #include "HMC5883L.h" 248 .................... #include "HMC5883L.h"
-   249 .................... // i2c slave addresses
225 .................... #define MAG_ADDR_R 0x3D //addresa pro cteni 250 .................... #define HMC5883L_WRT_ADDR 0x3C
226 .................... #define MAG_ADDR_W 0x3C 251 .................... #define HMC5883L_READ_ADDR 0x3D
227 .................... 252 ....................
-   253 .................... // Register addresses
-   254 .................... #define HMC5883L_CFG_A_REG 0x00
-   255 .................... #define HMC5883L_CFG_B_REG 0x01
-   256 .................... #define HMC5883L_MODE_REG 0x02
-   257 .................... #define HMC5883L_X_MSB_REG 0x03
228 .................... 258 ....................
229 .................... //Konstanty nastavujici rozsah 259 .................... //Konstanty nastavujici rozsah
230 .................... //pro void set_mag_roz (unsigned int8 h) 260 .................... //pro void set_mag_roz (unsigned int8 h)
231 .................... #define MAG_ROZ088 0x00 261 .................... #define MAG_ROZ088 0x00
232 .................... #define MAG_ROZ130 0x20 262 .................... #define MAG_ROZ130 0x20
233 .................... #define MAG_ROZ190 0x40 263 .................... #define MAG_ROZ190 0x40
234 .................... #define MAG_ROZ250 0x60 264 .................... #define MAG_ROZ250 0x60
235 .................... #define MAG_ROZ400 0x80 265 .................... #define MAG_ROZ400 0x80
236 .................... #define MAG_ROZ470 0xA0 266 .................... #define MAG_ROZ470 0xA0
237 .................... #define MAG_ROZ560 0xC0 267 .................... #define MAG_ROZ560 0xC0
238 .................... #define MAG_ROZ810 0xE0 268 .................... #define MAG_ROZ810 0xE0
239 .................... 269 ....................
240 .................... 270 ....................
241 .................... #include "HMC5883L.c" 271 .................... #include "HMC5883L.c"
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 .................... //------------------------------ 272 .................... //------------------------------
253 .................... // Low level routines 273 .................... // Low level routines
254 .................... //------------------------------ 274 .................... //------------------------------
255 .................... void hmc5883l_write_reg(int8 reg, int8 data) 275 .................... void hmc5883l_write_reg(int8 reg, int8 data)
256 .................... { 276 .................... {
257 .................... i2c_start(); 277 .................... i2c_start();
258 * 278 *
259 0069: BSF 20.4 279 00D3: BSF 20.4
260 006A: MOVF 20,W 280 00D4: MOVF 20,W
261 006B: BSF 03.5 281 00D5: BSF 03.5
262 006C: MOVWF 07 282 00D6: MOVWF 07
263 006D: NOP 283 00D7: NOP
264 006E: BCF 03.5 284 00D8: BCF 03.5
265 006F: BSF 20.3 285 00D9: BSF 20.3
266 0070: MOVF 20,W 286 00DA: MOVF 20,W
267 0071: BSF 03.5 287 00DB: BSF 03.5
268 0072: MOVWF 07 288 00DC: MOVWF 07
269 0073: NOP 289 00DD: NOP
270 0074: BCF 03.5 290 00DE: BCF 03.5
271 0075: BCF 07.4 291 00DF: BCF 07.4
272 0076: BCF 20.4 292 00E0: BCF 20.4
273 0077: MOVF 20,W 293 00E1: MOVF 20,W
274 0078: BSF 03.5 294 00E2: BSF 03.5
275 0079: MOVWF 07 295 00E3: MOVWF 07
276 007A: NOP 296 00E4: NOP
277 007B: BCF 03.5 297 00E5: BCF 03.5
278 007C: BCF 07.3 298 00E6: BCF 07.3
279 007D: BCF 20.3 299 00E7: BCF 20.3
280 007E: MOVF 20,W 300 00E8: MOVF 20,W
281 007F: BSF 03.5 301 00E9: BSF 03.5
282 0080: MOVWF 07 302 00EA: MOVWF 07
283 .................... i2c_write(HMC5883L_WRT_ADDR); 303 .................... i2c_write(HMC5883L_WRT_ADDR);
284 0081: MOVLW 3C 304 00EB: MOVLW 3C
285 0082: BCF 03.5 305 00EC: BCF 03.5
286 0083: MOVWF 3D 306 00ED: MOVWF 3D
287 0084: CALL 01F 307 00EE: CALL 089
288 .................... i2c_write(reg); 308 .................... i2c_write(reg);
289 0085: MOVF 37,W 309 00EF: MOVF 37,W
290 0086: MOVWF 3D 310 00F0: MOVWF 3D
291 0087: CALL 01F 311 00F1: CALL 089
292 .................... i2c_write(data); 312 .................... i2c_write(data);
293 0088: MOVF 38,W 313 00F2: MOVF 38,W
294 0089: MOVWF 3D 314 00F3: MOVWF 3D
295 008A: CALL 01F 315 00F4: CALL 089
296 .................... i2c_stop(); 316 .................... i2c_stop();
297 008B: BCF 20.4 317 00F5: BCF 20.4
298 008C: MOVF 20,W 318 00F6: MOVF 20,W
299 008D: BSF 03.5 319 00F7: BSF 03.5
300 008E: MOVWF 07 320 00F8: MOVWF 07
301 008F: NOP 321 00F9: NOP
302 0090: BCF 03.5 322 00FA: BCF 03.5
303 0091: BSF 20.3 323 00FB: BSF 20.3
304 0092: MOVF 20,W 324 00FC: MOVF 20,W
305 0093: BSF 03.5 325 00FD: BSF 03.5
306 0094: MOVWF 07 326 00FE: MOVWF 07
307 0095: BCF 03.5 327 00FF: BCF 03.5
308 0096: BTFSS 07.3 328 0100: BTFSS 07.3
309 0097: GOTO 096 329 0101: GOTO 100
310 0098: NOP 330 0102: NOP
311 0099: GOTO 09A 331 0103: GOTO 104
312 009A: NOP 332 0104: NOP
313 009B: BSF 20.4 333 0105: BSF 20.4
314 009C: MOVF 20,W 334 0106: MOVF 20,W
315 009D: BSF 03.5 335 0107: BSF 03.5
316 009E: MOVWF 07 336 0108: MOVWF 07
317 009F: NOP 337 0109: NOP
318 .................... } 338 .................... }
319 00A0: BCF 03.5 339 010A: BCF 03.5
320 00A1: RETURN 340 010B: RETURN
321 .................... 341 ....................
322 .................... //------------------------------ 342 .................... //------------------------------
323 .................... int8 hmc5883l_read_reg(int8 reg) 343 .................... int8 hmc5883l_read_reg(int8 reg)
324 .................... { 344 .................... {
325 .................... int8 retval; 345 .................... int8 retval;
326 .................... 346 ....................
327 .................... i2c_start(); 347 .................... i2c_start();
328 .................... i2c_write(HMC5883L_WRT_ADDR); 348 .................... i2c_write(HMC5883L_WRT_ADDR);
329 .................... i2c_write(reg); 349 .................... i2c_write(reg);
330 .................... i2c_start(); 350 .................... i2c_start();
331 .................... i2c_write(HMC5883L_READ_ADDR); 351 .................... i2c_write(HMC5883L_READ_ADDR);
332 .................... retval = i2c_read(0); 352 .................... retval = i2c_read(0);
333 .................... i2c_stop(); 353 .................... i2c_stop();
334 .................... 354 ....................
335 .................... return(retval); 355 .................... return(retval);
336 .................... } 356 .................... }
337 .................... 357 ....................
338 .................... //------------------------------ 358 .................... //------------------------------
339 .................... typedef struct 359 .................... typedef struct
340 .................... { 360 .................... {
341 .................... signed int16 x; 361 .................... signed int16 x;
342 .................... signed int16 y; 362 .................... signed int16 y;
343 .................... signed int16 z; 363 .................... signed int16 z;
344 .................... }hmc5883l_result; 364 .................... }hmc5883l_result;
345 .................... 365 ....................
346 .................... // This global structure holds the values read 366 .................... // This global structure holds the values read
347 .................... // from the HMC5883L x,y,z registers. 367 .................... // from the HMC5883L x,y,z registers.
348 .................... hmc5883l_result compass = {0,0,0}; 368 .................... hmc5883l_result compass = {0,0,0};
349 * 369 *
350 0270: CLRF 21 370 0287: CLRF 21
351 0271: CLRF 22 371 0288: CLRF 22
352 0272: CLRF 23 372 0289: CLRF 23
353 0273: CLRF 24 373 028A: CLRF 24
354 0274: CLRF 25 374 028B: CLRF 25
355 0275: CLRF 26 375 028C: CLRF 26
356 .................... 376 ....................
357 .................... //------------------------------ 377 .................... //------------------------------
358 .................... void hmc5883l_read_data(void) 378 .................... void hmc5883l_read_data(void)
359 .................... { 379 .................... {
360 .................... unsigned int8 x_lsb; 380 .................... unsigned int8 x_lsb;
361 .................... unsigned int8 x_msb; 381 .................... unsigned int8 x_msb;
362 .................... 382 ....................
363 .................... unsigned int8 y_lsb; 383 .................... unsigned int8 y_lsb;
364 .................... unsigned int8 y_msb; 384 .................... unsigned int8 y_msb;
365 .................... 385 ....................
366 .................... unsigned int8 z_lsb; 386 .................... unsigned int8 z_lsb;
367 .................... unsigned int8 z_msb; 387 .................... unsigned int8 z_msb;
368 .................... 388 ....................
369 .................... i2c_start(); 389 .................... i2c_start();
370 * 390 *
371 0150: BSF 20.4 391 0151: BSF 20.4
372 0151: MOVF 20,W 392 0152: MOVF 20,W
373 0152: BSF 03.5 393 0153: BSF 03.5
374 0153: MOVWF 07 394 0154: MOVWF 07
375 0154: NOP 395 0155: NOP
376 0155: BCF 03.5 396 0156: BCF 03.5
377 0156: BSF 20.3 397 0157: BSF 20.3
378 0157: MOVF 20,W 398 0158: MOVF 20,W
379 0158: BSF 03.5 399 0159: BSF 03.5
380 0159: MOVWF 07 400 015A: MOVWF 07
381 015A: NOP 401 015B: NOP
382 015B: BCF 03.5 402 015C: BCF 03.5
383 015C: BCF 07.4 403 015D: BCF 07.4
384 015D: BCF 20.4 404 015E: BCF 20.4
385 015E: MOVF 20,W 405 015F: MOVF 20,W
386 015F: BSF 03.5 406 0160: BSF 03.5
387 0160: MOVWF 07 407 0161: MOVWF 07
388 0161: NOP 408 0162: NOP
389 0162: BCF 03.5 409 0163: BCF 03.5
390 0163: BCF 07.3 410 0164: BCF 07.3
391 0164: BCF 20.3 411 0165: BCF 20.3
392 0165: MOVF 20,W 412 0166: MOVF 20,W
393 0166: BSF 03.5 413 0167: BSF 03.5
394 0167: MOVWF 07 414 0168: MOVWF 07
395 .................... i2c_write(HMC5883L_WRT_ADDR); 415 .................... i2c_write(HMC5883L_WRT_ADDR);
396 0168: MOVLW 3C 416 0169: MOVLW 3C
397 0169: BCF 03.5 417 016A: BCF 03.5
398 016A: MOVWF 3D 418 016B: MOVWF 3D
399 016B: CALL 01F 419 016C: CALL 089
400 .................... i2c_write(HMC5883L_X_MSB_REG); // Point to X-msb register 420 .................... i2c_write(HMC5883L_X_MSB_REG); // Point to X-msb register
401 016C: MOVLW 03 421 016D: MOVLW 03
402 016D: MOVWF 3D 422 016E: MOVWF 3D
403 016E: CALL 01F 423 016F: CALL 089
404 .................... i2c_start(); 424 .................... i2c_start();
405 016F: BSF 20.4 425 0170: BSF 20.4
406 0170: MOVF 20,W 426 0171: MOVF 20,W
407 0171: BSF 03.5 427 0172: BSF 03.5
408 0172: MOVWF 07 428 0173: MOVWF 07
409 0173: NOP 429 0174: NOP
410 0174: BCF 03.5 430 0175: BCF 03.5
411 0175: BSF 20.3 431 0176: BSF 20.3
412 0176: MOVF 20,W 432 0177: MOVF 20,W
413 0177: BSF 03.5 433 0178: BSF 03.5
414 0178: MOVWF 07 434 0179: MOVWF 07
415 0179: NOP 435 017A: NOP
416 017A: BCF 03.5 436 017B: BCF 03.5
417 017B: BTFSS 07.3 437 017C: BTFSS 07.3
418 017C: GOTO 17B 438 017D: GOTO 17C
419 017D: BCF 07.4 439 017E: BCF 07.4
420 017E: BCF 20.4 440 017F: BCF 20.4
421 017F: MOVF 20,W 441 0180: MOVF 20,W
422 0180: BSF 03.5 442 0181: BSF 03.5
423 0181: MOVWF 07 443 0182: MOVWF 07
424 0182: NOP 444 0183: NOP
425 0183: BCF 03.5 445 0184: BCF 03.5
426 0184: BCF 07.3 446 0185: BCF 07.3
427 0185: BCF 20.3 447 0186: BCF 20.3
428 0186: MOVF 20,W 448 0187: MOVF 20,W
429 0187: BSF 03.5 449 0188: BSF 03.5
430 0188: MOVWF 07 450 0189: MOVWF 07
431 .................... i2c_write(HMC5883L_READ_ADDR); 451 .................... i2c_write(HMC5883L_READ_ADDR);
432 0189: MOVLW 3D 452 018A: MOVLW 3D
433 018A: BCF 03.5 453 018B: BCF 03.5
434 018B: MOVWF 3D 454 018C: MOVWF 3D
435 018C: CALL 01F 455 018D: CALL 089
436 .................... 456 ....................
437 .................... x_msb = i2c_read(); 457 .................... x_msb = i2c_read();
438 018D: MOVLW 01 458 018E: MOVLW 01
439 018E: MOVWF 77 459 018F: MOVWF 77
440 018F: CALL 10B 460 0190: CALL 10C
441 0190: MOVF 78,W 461 0191: MOVF 78,W
442 0191: MOVWF 38 462 0192: MOVWF 38
443 .................... x_lsb = i2c_read(); 463 .................... x_lsb = i2c_read();
444 0192: MOVLW 01 464 0193: MOVLW 01
445 0193: MOVWF 77 465 0194: MOVWF 77
446 0194: CALL 10B 466 0195: CALL 10C
447 0195: MOVF 78,W 467 0196: MOVF 78,W
448 0196: MOVWF 37 468 0197: MOVWF 37
449 .................... 469 ....................
450 .................... z_msb = i2c_read(); 470 .................... z_msb = i2c_read();
451 0197: MOVLW 01 471 0198: MOVLW 01
452 0198: MOVWF 77 472 0199: MOVWF 77
453 0199: CALL 10B 473 019A: CALL 10C
454 019A: MOVF 78,W 474 019B: MOVF 78,W
455 019B: MOVWF 3C 475 019C: MOVWF 3C
456 .................... z_lsb = i2c_read(); 476 .................... z_lsb = i2c_read();
457 019C: MOVLW 01 477 019D: MOVLW 01
458 019D: MOVWF 77 478 019E: MOVWF 77
459 019E: CALL 10B 479 019F: CALL 10C
460 019F: MOVF 78,W 480 01A0: MOVF 78,W
461 01A0: MOVWF 3B 481 01A1: MOVWF 3B
462 .................... 482 ....................
463 .................... y_msb = i2c_read(); 483 .................... y_msb = i2c_read();
464 01A1: MOVLW 01 484 01A2: MOVLW 01
465 01A2: MOVWF 77 485 01A3: MOVWF 77
466 01A3: CALL 10B 486 01A4: CALL 10C
467 01A4: MOVF 78,W 487 01A5: MOVF 78,W
468 01A5: MOVWF 3A 488 01A6: MOVWF 3A
469 .................... y_lsb = i2c_read(0); // do a NACK on last read 489 .................... y_lsb = i2c_read(0); // do a NACK on last read
470 01A6: CLRF 77 490 01A7: CLRF 77
471 01A7: CALL 10B 491 01A8: CALL 10C
472 01A8: MOVF 78,W 492 01A9: MOVF 78,W
473 01A9: MOVWF 39 493 01AA: MOVWF 39
474 .................... 494 ....................
475 .................... i2c_stop(); 495 .................... i2c_stop();
476 01AA: BCF 20.4 496 01AB: BCF 20.4
477 01AB: MOVF 20,W 497 01AC: MOVF 20,W
478 01AC: BSF 03.5 498 01AD: BSF 03.5
479 01AD: MOVWF 07 499 01AE: MOVWF 07
480 01AE: NOP 500 01AF: NOP
481 01AF: BCF 03.5 501 01B0: BCF 03.5
482 01B0: BSF 20.3 502 01B1: BSF 20.3
483 01B1: MOVF 20,W 503 01B2: MOVF 20,W
484 01B2: BSF 03.5 504 01B3: BSF 03.5
485 01B3: MOVWF 07 505 01B4: MOVWF 07
486 01B4: BCF 03.5 506 01B5: BCF 03.5
487 01B5: BTFSS 07.3 507 01B6: BTFSS 07.3
488 01B6: GOTO 1B5 508 01B7: GOTO 1B6
489 01B7: NOP 509 01B8: NOP
490 01B8: GOTO 1B9 510 01B9: GOTO 1BA
491 01B9: NOP 511 01BA: NOP
492 01BA: BSF 20.4 512 01BB: BSF 20.4
493 01BB: MOVF 20,W 513 01BC: MOVF 20,W
494 01BC: BSF 03.5 514 01BD: BSF 03.5
495 01BD: MOVWF 07 515 01BE: MOVWF 07
496 01BE: NOP 516 01BF: NOP
497 .................... 517 ....................
498 .................... // Combine high and low bytes into 16-bit values. 518 .................... // Combine high and low bytes into 16-bit values.
499 .................... compass.x = make16(x_msb, x_lsb); 519 .................... compass.x = make16(x_msb, x_lsb);
500 01BF: BCF 03.5 520 01C0: BCF 03.5
501 01C0: MOVF 38,W 521 01C1: MOVF 38,W
502 01C1: MOVWF 22 522 01C2: MOVWF 22
503 01C2: MOVF 37,W 523 01C3: MOVF 37,W
504 01C3: MOVWF 21 524 01C4: MOVWF 21
505 .................... compass.y = make16(y_msb, y_lsb); 525 .................... compass.y = make16(y_msb, y_lsb);
506 01C4: MOVF 3A,W 526 01C5: MOVF 3A,W
507 01C5: MOVWF 24 527 01C6: MOVWF 24
508 01C6: MOVF 39,W 528 01C7: MOVF 39,W
509 01C7: MOVWF 23 529 01C8: MOVWF 23
510 .................... compass.z = make16(z_msb, z_lsb); 530 .................... compass.z = make16(z_msb, z_lsb);
511 01C8: MOVF 3C,W 531 01C9: MOVF 3C,W
512 01C9: MOVWF 26 532 01CA: MOVWF 26
513 01CA: MOVF 3B,W 533 01CB: MOVF 3B,W
514 01CB: MOVWF 25 534 01CC: MOVWF 25
515 .................... } 535 .................... }
516 01CC: BCF 0A.3 536 01CD: BCF 0A.3
517 01CD: BCF 0A.4 537 01CE: BCF 0A.4
518 01CE: GOTO 2E1 (RETURN) 538 01CF: GOTO 2F6 (RETURN)
519 .................... 539 ....................
520 .................... 540 ....................
521 .................... 541 ....................
522 .................... 542 ....................
523 .................... #include <math.h> 543 .................... #include <math.h>
524 .................... //////////////////////////////////////////////////////////////////////////// 544 .................... ////////////////////////////////////////////////////////////////////////////
525 .................... //// (C) Copyright 1996,2008 Custom Computer Services //// 545 .................... //// (C) Copyright 1996,2008 Custom Computer Services ////
526 .................... //// This source code may only be used by licensed users of the CCS C //// 546 .................... //// 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 //// 547 .................... //// compiler. This source code may only be distributed to other ////
528 .................... //// licensed users of the CCS C compiler. No other use, reproduction //// 548 .................... //// licensed users of the CCS C compiler. No other use, reproduction ////
529 .................... //// or distribution is permitted without written permission. //// 549 .................... //// or distribution is permitted without written permission. ////
530 .................... //// Derivative programs created using this software in object code //// 550 .................... //// Derivative programs created using this software in object code ////
531 .................... //// form are not restricted in any way. //// 551 .................... //// form are not restricted in any way. ////
532 .................... //////////////////////////////////////////////////////////////////////////// 552 .................... ////////////////////////////////////////////////////////////////////////////
533 .................... //// //// 553 .................... //// ////
534 .................... //// History: //// 554 .................... //// History: ////
535 .................... //// * 9/20/2001 : Improvments are made to sin/cos code. //// 555 .................... //// * 9/20/2001 : Improvments are made to sin/cos code. ////
536 .................... //// The code now is small, much faster, //// 556 .................... //// The code now is small, much faster, ////
537 .................... //// and more accurate. //// 557 .................... //// and more accurate. ////
538 .................... //// * 2/21/2007 : Compiler handles & operator differently and does 558 .................... //// * 2/21/2007 : Compiler handles & operator differently and does
539 .................... //// not return generic (int8 *) so type cast is done //// 559 .................... //// not return generic (int8 *) so type cast is done ////
540 .................... //// //// 560 .................... //// ////
541 .................... //////////////////////////////////////////////////////////////////////////// 561 .................... ////////////////////////////////////////////////////////////////////////////
542 .................... 562 ....................
543 .................... #ifndef MATH_H 563 .................... #ifndef MATH_H
544 .................... #define MATH_H 564 .................... #define MATH_H
545 .................... 565 ....................
546 .................... #ifdef PI 566 .................... #ifdef PI
547 .................... #undef PI 567 .................... #undef PI
548 .................... #endif 568 .................... #endif
549 .................... #define PI 3.1415926535897932 569 .................... #define PI 3.1415926535897932
550 .................... 570 ....................
551 .................... 571 ....................
552 .................... #define SQRT2 1.4142135623730950 572 .................... #define SQRT2 1.4142135623730950
553 .................... 573 ....................
554 .................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279}; 574 .................... //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}; 575 .................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718};
556 .................... 576 ....................
557 .................... ///////////////////////////// Round Functions ////////////////////////////// 577 .................... ///////////////////////////// Round Functions //////////////////////////////
558 .................... 578 ....................
559 .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n) 579 .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n)
560 .................... { 580 .................... {
561 .................... float32 y, res; 581 .................... float32 y, res;
562 .................... unsigned int16 l; 582 .................... unsigned int16 l;
563 .................... int1 s; 583 .................... int1 s;
564 .................... 584 ....................
565 .................... s = 0; 585 .................... s = 0;
566 .................... y = x; 586 .................... y = x;
567 .................... 587 ....................
568 .................... if (x < 0) 588 .................... if (x < 0)
569 .................... { 589 .................... {
570 .................... s = 1; 590 .................... s = 1;
571 .................... y = -y; 591 .................... y = -y;
572 .................... } 592 .................... }
573 .................... 593 ....................
574 .................... if (y <= 32768.0) 594 .................... if (y <= 32768.0)
575 .................... res = (float32)(unsigned int16)y; 595 .................... res = (float32)(unsigned int16)y;
576 .................... 596 ....................
577 .................... else if (y < 10000000.0) 597 .................... else if (y < 10000000.0)
578 .................... { 598 .................... {
579 .................... l = (unsigned int16)(y/32768.0); 599 .................... l = (unsigned int16)(y/32768.0);
580 .................... y = 32768.0*(y/32768.0 - (float32)l); 600 .................... y = 32768.0*(y/32768.0 - (float32)l);
581 .................... res = 32768.0*(float32)l; 601 .................... res = 32768.0*(float32)l;
582 .................... res += (float32)(unsigned int16)y; 602 .................... res += (float32)(unsigned int16)y;
583 .................... } 603 .................... }
584 .................... 604 ....................
585 .................... else 605 .................... else
586 .................... res = y; 606 .................... res = y;
587 .................... 607 ....................
588 .................... y = y - (float32)(unsigned int16)y; 608 .................... y = y - (float32)(unsigned int16)y;
589 .................... 609 ....................
590 .................... if (s) 610 .................... if (s)
591 .................... res = -res; 611 .................... res = -res;
592 .................... 612 ....................
593 .................... if (y != 0) 613 .................... if (y != 0)
594 .................... { 614 .................... {
595 .................... if (s == 1 && n == 0) 615 .................... if (s == 1 && n == 0)
596 .................... res -= 1.0; 616 .................... res -= 1.0;
597 .................... 617 ....................
598 .................... if (s == 0 && n == 1) 618 .................... if (s == 0 && n == 1)
599 .................... res += 1.0; 619 .................... res += 1.0;
600 .................... } 620 .................... }
601 .................... if (x == 0) 621 .................... if (x == 0)
602 .................... res = 0; 622 .................... res = 0;
603 .................... 623 ....................
604 .................... return (res); 624 .................... return (res);
605 .................... } 625 .................... }
606 .................... 626 ....................
607 .................... // Overloaded Functions to take care for new Data types in PCD 627 .................... // Overloaded Functions to take care for new Data types in PCD
608 .................... // Overloaded function CEIL_FLOOR() for data type - Float48 628 .................... // Overloaded function CEIL_FLOOR() for data type - Float48
609 .................... #if defined(__PCD__) 629 .................... #if defined(__PCD__)
610 .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n) 630 .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n)
611 .................... { 631 .................... {
612 .................... float48 y, res; 632 .................... float48 y, res;
613 .................... unsigned int16 l; 633 .................... unsigned int16 l;
614 .................... int1 s; 634 .................... int1 s;
615 .................... 635 ....................
616 .................... s = 0; 636 .................... s = 0;
617 .................... y = x; 637 .................... y = x;
618 .................... 638 ....................
619 .................... if (x < 0) 639 .................... if (x < 0)
620 .................... { 640 .................... {
621 .................... s = 1; 641 .................... s = 1;
622 .................... y = -y; 642 .................... y = -y;
623 .................... } 643 .................... }
624 .................... 644 ....................
625 .................... if (y <= 32768.0) 645 .................... if (y <= 32768.0)
626 .................... res = (float48)(unsigned int16)y; 646 .................... res = (float48)(unsigned int16)y;
627 .................... 647 ....................
628 .................... else if (y < 10000000.0) 648 .................... else if (y < 10000000.0)
629 .................... { 649 .................... {
630 .................... l = (unsigned int16)(y/32768.0); 650 .................... l = (unsigned int16)(y/32768.0);
631 .................... y = 32768.0*(y/32768.0 - (float48)l); 651 .................... y = 32768.0*(y/32768.0 - (float48)l);
632 .................... res = 32768.0*(float32)l; 652 .................... res = 32768.0*(float32)l;
633 .................... res += (float48)(unsigned int16)y; 653 .................... res += (float48)(unsigned int16)y;
634 .................... } 654 .................... }
635 .................... 655 ....................
636 .................... else 656 .................... else
637 .................... res = y; 657 .................... res = y;
638 .................... 658 ....................
639 .................... y = y - (float48)(unsigned int16)y; 659 .................... y = y - (float48)(unsigned int16)y;
640 .................... 660 ....................
641 .................... if (s) 661 .................... if (s)
642 .................... res = -res; 662 .................... res = -res;
643 .................... 663 ....................
644 .................... if (y != 0) 664 .................... if (y != 0)
645 .................... { 665 .................... {
646 .................... if (s == 1 && n == 0) 666 .................... if (s == 1 && n == 0)
647 .................... res -= 1.0; 667 .................... res -= 1.0;
648 .................... 668 ....................
649 .................... if (s == 0 && n == 1) 669 .................... if (s == 0 && n == 1)
650 .................... res += 1.0; 670 .................... res += 1.0;
651 .................... } 671 .................... }
652 .................... if (x == 0) 672 .................... if (x == 0)
653 .................... res = 0; 673 .................... res = 0;
654 .................... 674 ....................
655 .................... return (res); 675 .................... return (res);
656 .................... } 676 .................... }
657 .................... 677 ....................
658 .................... 678 ....................
659 .................... // Overloaded function CEIL_FLOOR() for data type - Float64 679 .................... // Overloaded function CEIL_FLOOR() for data type - Float64
660 .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n) 680 .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n)
661 .................... { 681 .................... {
662 .................... float64 y, res; 682 .................... float64 y, res;
663 .................... unsigned int16 l; 683 .................... unsigned int16 l;
664 .................... int1 s; 684 .................... int1 s;
665 .................... 685 ....................
666 .................... s = 0; 686 .................... s = 0;
667 .................... y = x; 687 .................... y = x;
668 .................... 688 ....................
669 .................... if (x < 0) 689 .................... if (x < 0)
670 .................... { 690 .................... {
671 .................... s = 1; 691 .................... s = 1;
672 .................... y = -y; 692 .................... y = -y;
673 .................... } 693 .................... }
674 .................... 694 ....................
675 .................... if (y <= 32768.0) 695 .................... if (y <= 32768.0)
676 .................... res = (float64)(unsigned int16)y; 696 .................... res = (float64)(unsigned int16)y;
677 .................... 697 ....................
678 .................... else if (y < 10000000.0) 698 .................... else if (y < 10000000.0)
679 .................... { 699 .................... {
680 .................... l = (unsigned int16)(y/32768.0); 700 .................... l = (unsigned int16)(y/32768.0);
681 .................... y = 32768.0*(y/32768.0 - (float64)l); 701 .................... y = 32768.0*(y/32768.0 - (float64)l);
682 .................... res = 32768.0*(float64)l; 702 .................... res = 32768.0*(float64)l;
683 .................... res += (float64)(unsigned int16)y; 703 .................... res += (float64)(unsigned int16)y;
684 .................... } 704 .................... }
685 .................... 705 ....................
686 .................... else 706 .................... else
687 .................... res = y; 707 .................... res = y;
688 .................... 708 ....................
689 .................... y = y - (float64)(unsigned int16)y; 709 .................... y = y - (float64)(unsigned int16)y;
690 .................... 710 ....................
691 .................... if (s) 711 .................... if (s)
692 .................... res = -res; 712 .................... res = -res;
693 .................... 713 ....................
694 .................... if (y != 0) 714 .................... if (y != 0)
695 .................... { 715 .................... {
696 .................... if (s == 1 && n == 0) 716 .................... if (s == 1 && n == 0)
697 .................... res -= 1.0; 717 .................... res -= 1.0;
698 .................... 718 ....................
699 .................... if (s == 0 && n == 1) 719 .................... if (s == 0 && n == 1)
700 .................... res += 1.0; 720 .................... res += 1.0;
701 .................... } 721 .................... }
702 .................... if (x == 0) 722 .................... if (x == 0)
703 .................... res = 0; 723 .................... res = 0;
704 .................... 724 ....................
705 .................... return (res); 725 .................... return (res);
706 .................... } 726 .................... }
707 .................... #endif 727 .................... #endif
708 .................... 728 ....................
709 .................... //////////////////////////////////////////////////////////////////////////// 729 .................... ////////////////////////////////////////////////////////////////////////////
710 .................... // float floor(float x) 730 .................... // float floor(float x)
711 .................... //////////////////////////////////////////////////////////////////////////// 731 .................... ////////////////////////////////////////////////////////////////////////////
712 .................... // Description : rounds down the number x. 732 .................... // Description : rounds down the number x.
713 .................... // Date : N/A 733 .................... // Date : N/A
714 .................... // 734 .................... //
715 .................... float32 floor(float32 x) 735 .................... float32 floor(float32 x)
716 .................... { 736 .................... {
717 .................... return CEIL_FLOOR(x, 0); 737 .................... return CEIL_FLOOR(x, 0);
718 .................... } 738 .................... }
719 .................... // Following 2 functions are overloaded functions of floor() for PCD 739 .................... // Following 2 functions are overloaded functions of floor() for PCD
720 .................... // Overloaded function floor() for data type - Float48 740 .................... // Overloaded function floor() for data type - Float48
721 .................... #if defined(__PCD__) 741 .................... #if defined(__PCD__)
722 .................... float48 floor(float48 x) 742 .................... float48 floor(float48 x)
723 .................... { 743 .................... {
724 .................... return CEIL_FLOOR(x, 0); 744 .................... return CEIL_FLOOR(x, 0);
725 .................... } 745 .................... }
726 .................... 746 ....................
727 .................... // Overloaded function floor() for data type - Float64 747 .................... // Overloaded function floor() for data type - Float64
728 .................... float64 floor(float64 x) 748 .................... float64 floor(float64 x)
729 .................... { 749 .................... {
730 .................... return CEIL_FLOOR(x, 0); 750 .................... return CEIL_FLOOR(x, 0);
731 .................... } 751 .................... }
732 .................... #endif 752 .................... #endif
733 .................... 753 ....................
734 .................... 754 ....................
735 .................... //////////////////////////////////////////////////////////////////////////// 755 .................... ////////////////////////////////////////////////////////////////////////////
736 .................... // float ceil(float x) 756 .................... // float ceil(float x)
737 .................... //////////////////////////////////////////////////////////////////////////// 757 .................... ////////////////////////////////////////////////////////////////////////////
738 .................... // Description : rounds up the number x. 758 .................... // Description : rounds up the number x.
739 .................... // Date : N/A 759 .................... // Date : N/A
740 .................... // 760 .................... //
741 .................... float32 ceil(float32 x) 761 .................... float32 ceil(float32 x)
742 .................... { 762 .................... {
743 .................... return CEIL_FLOOR(x, 1); 763 .................... return CEIL_FLOOR(x, 1);
744 .................... } 764 .................... }
745 .................... // Following 2 functions are overloaded functions of ceil() for PCD 765 .................... // Following 2 functions are overloaded functions of ceil() for PCD
746 .................... // Overloaded function ceil() for data type - Float48 766 .................... // Overloaded function ceil() for data type - Float48
747 .................... #if defined(__PCD__) 767 .................... #if defined(__PCD__)
748 .................... float48 ceil(float48 x) 768 .................... float48 ceil(float48 x)
749 .................... { 769 .................... {
750 .................... return CEIL_FLOOR(x, 1); 770 .................... return CEIL_FLOOR(x, 1);
751 .................... } 771 .................... }
752 .................... 772 ....................
753 .................... // Overloaded function ceil() for data type - Float64 773 .................... // Overloaded function ceil() for data type - Float64
754 .................... float64 ceil(float64 x) 774 .................... float64 ceil(float64 x)
755 .................... { 775 .................... {
756 .................... return CEIL_FLOOR(x, 1); 776 .................... return CEIL_FLOOR(x, 1);
757 .................... } 777 .................... }
758 .................... #endif 778 .................... #endif
759 .................... 779 ....................
760 .................... //////////////////////////////////////////////////////////////////////////// 780 .................... ////////////////////////////////////////////////////////////////////////////
761 .................... // float fabs(float x) 781 .................... // float fabs(float x)
762 .................... //////////////////////////////////////////////////////////////////////////// 782 .................... ////////////////////////////////////////////////////////////////////////////
763 .................... // Description : Computes the absolute value of floating point number x 783 .................... // Description : Computes the absolute value of floating point number x
764 .................... // Returns : returns the absolute value of x 784 .................... // Returns : returns the absolute value of x
765 .................... // Date : N/A 785 .................... // Date : N/A
766 .................... // 786 .................... //
767 .................... #define fabs abs 787 .................... #define fabs abs
768 .................... 788 ....................
769 .................... //////////////////////////////////////////////////////////////////////////// 789 .................... ////////////////////////////////////////////////////////////////////////////
770 .................... // float fmod(float x) 790 .................... // float fmod(float x)
771 .................... //////////////////////////////////////////////////////////////////////////// 791 .................... ////////////////////////////////////////////////////////////////////////////
772 .................... // Description : Computes the floating point remainder of x/y 792 .................... // 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 793 .................... // 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 794 .................... // 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. 795 .................... // magnitude of y. If y is zero then a domain error occurs.
776 .................... // Date : N/A 796 .................... // Date : N/A
777 .................... // 797 .................... //
778 .................... 798 ....................
779 .................... float fmod(float32 x,float32 y) 799 .................... float fmod(float32 x,float32 y)
780 .................... { 800 .................... {
781 .................... float32 i; 801 .................... float32 i;
782 .................... if (y!=0.0) 802 .................... if (y!=0.0)
783 .................... { 803 .................... {
784 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 804 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
785 .................... return(x-(i*y)); 805 .................... return(x-(i*y));
786 .................... } 806 .................... }
787 .................... else 807 .................... else
788 .................... { 808 .................... {
789 .................... #ifdef _ERRNO 809 .................... #ifdef _ERRNO
790 .................... { 810 .................... {
791 .................... errno=EDOM; 811 .................... errno=EDOM;
792 .................... } 812 .................... }
793 .................... #endif 813 .................... #endif
794 .................... } 814 .................... }
795 .................... } 815 .................... }
796 .................... //Overloaded function for fmod() for PCD 816 .................... //Overloaded function for fmod() for PCD
797 .................... // Overloaded function fmod() for data type - Float48 817 .................... // Overloaded function fmod() for data type - Float48
798 .................... #if defined(__PCD__) 818 .................... #if defined(__PCD__)
799 .................... float48 fmod(float48 x,float48 y) 819 .................... float48 fmod(float48 x,float48 y)
800 .................... { 820 .................... {
801 .................... float48 i; 821 .................... float48 i;
802 .................... if (y!=0.0) 822 .................... if (y!=0.0)
803 .................... { 823 .................... {
804 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 824 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
805 .................... return(x-(i*y)); 825 .................... return(x-(i*y));
806 .................... } 826 .................... }
807 .................... else 827 .................... else
808 .................... { 828 .................... {
809 .................... #ifdef _ERRNO 829 .................... #ifdef _ERRNO
810 .................... { 830 .................... {
811 .................... errno=EDOM; 831 .................... errno=EDOM;
812 .................... } 832 .................... }
813 .................... #endif 833 .................... #endif
814 .................... } 834 .................... }
815 .................... } 835 .................... }
816 .................... // Overloaded function fmod() for data type - Float64 836 .................... // Overloaded function fmod() for data type - Float64
817 .................... float64 fmod(float64 x,float64 y) 837 .................... float64 fmod(float64 x,float64 y)
818 .................... { 838 .................... {
819 .................... float64 i; 839 .................... float64 i;
820 .................... if (y!=0.0) 840 .................... if (y!=0.0)
821 .................... { 841 .................... {
822 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); 842 .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y);
823 .................... return(x-(i*y)); 843 .................... return(x-(i*y));
824 .................... } 844 .................... }
825 .................... else 845 .................... else
826 .................... { 846 .................... {
827 .................... #ifdef _ERRNO 847 .................... #ifdef _ERRNO
828 .................... { 848 .................... {
829 .................... errno=EDOM; 849 .................... errno=EDOM;
830 .................... } 850 .................... }
831 .................... #endif 851 .................... #endif
832 .................... } 852 .................... }
833 .................... } 853 .................... }
834 .................... #endif 854 .................... #endif
835 .................... //////////////////// Exponential and logarithmic functions //////////////////// 855 .................... //////////////////// Exponential and logarithmic functions ////////////////////
836 .................... //////////////////////////////////////////////////////////////////////////// 856 .................... ////////////////////////////////////////////////////////////////////////////
837 .................... // float exp(float x) 857 .................... // float exp(float x)
838 .................... //////////////////////////////////////////////////////////////////////////// 858 .................... ////////////////////////////////////////////////////////////////////////////
839 .................... // Description : returns the value (e^x) 859 .................... // Description : returns the value (e^x)
840 .................... // Date : N/A 860 .................... // Date : N/A
841 .................... // 861 .................... //
842 .................... #define LN2 0.6931471805599453 862 .................... #define LN2 0.6931471805599453
843 .................... 863 ....................
844 .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, 864 .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093,
845 .................... 0.0554965651, 0.240227138, 0.693147172}; 865 .................... 0.0554965651, 0.240227138, 0.693147172};
846 .................... 866 ....................
847 .................... 867 ....................
848 .................... float32 exp(float32 x) 868 .................... float32 exp(float32 x)
849 .................... { 869 .................... {
850 .................... float32 y, res, r; 870 .................... float32 y, res, r;
851 .................... #if defined(__PCD__) 871 .................... #if defined(__PCD__)
852 .................... int8 data1; 872 .................... int8 data1;
853 .................... #endif 873 .................... #endif
854 .................... signed int8 n; 874 .................... signed int8 n;
855 .................... int1 s; 875 .................... int1 s;
856 .................... #ifdef _ERRNO 876 .................... #ifdef _ERRNO
857 .................... if(x > 88.722838) 877 .................... if(x > 88.722838)
858 .................... { 878 .................... {
859 .................... errno=ERANGE; 879 .................... errno=ERANGE;
860 .................... return(0); 880 .................... return(0);
861 .................... } 881 .................... }
862 .................... #endif 882 .................... #endif
863 .................... n = (signed int16)(x/LN2); 883 .................... n = (signed int16)(x/LN2);
864 .................... s = 0; 884 .................... s = 0;
865 .................... y = x; 885 .................... y = x;
866 .................... 886 ....................
867 .................... if (x < 0) 887 .................... if (x < 0)
868 .................... { 888 .................... {
869 .................... s = 1; 889 .................... s = 1;
870 .................... n = -n; 890 .................... n = -n;
871 .................... y = -y; 891 .................... y = -y;
872 .................... } 892 .................... }
873 .................... 893 ....................
874 .................... res = 0.0; 894 .................... res = 0.0;
875 .................... #if !defined(__PCD__) 895 .................... #if !defined(__PCD__)
876 .................... *((unsigned int8 *)(&res)) = n + 0x7F; 896 .................... *((unsigned int8 *)(&res)) = n + 0x7F;
877 .................... #endif 897 .................... #endif
878 .................... 898 ....................
879 .................... #if defined(__PCD__) // Takes care of IEEE format for PCD 899 .................... #if defined(__PCD__) // Takes care of IEEE format for PCD
880 .................... data1 = n+0x7F; 900 .................... data1 = n+0x7F;
881 .................... if(bit_test(data1,0)) 901 .................... if(bit_test(data1,0))
882 .................... bit_set(*(((unsigned int8 *)(&res)+2)),7); 902 .................... bit_set(*(((unsigned int8 *)(&res)+2)),7);
883 .................... rotate_right(&data1,1); 903 .................... rotate_right(&data1,1);
884 .................... bit_clear(data1,7); 904 .................... bit_clear(data1,7);
885 .................... *(((unsigned int8 *)(&res)+3)) = data1; 905 .................... *(((unsigned int8 *)(&res)+3)) = data1;
886 .................... #endif 906 .................... #endif
887 .................... 907 ....................
888 .................... y = y/LN2 - (float32)n; 908 .................... y = y/LN2 - (float32)n;
889 .................... 909 ....................
890 .................... r = pe[0]*y + pe[1]; 910 .................... r = pe[0]*y + pe[1];
891 .................... r = r*y + pe[2]; 911 .................... r = r*y + pe[2];
892 .................... r = r*y + pe[3]; 912 .................... r = r*y + pe[3];
893 .................... r = r*y + pe[4]; 913 .................... r = r*y + pe[4];
894 .................... r = r*y + pe[5]; 914 .................... r = r*y + pe[5];
895 .................... 915 ....................
896 .................... res = res*(1.0 + y*r); 916 .................... res = res*(1.0 + y*r);
897 .................... 917 ....................
898 .................... if (s) 918 .................... if (s)
899 .................... res = 1.0/res; 919 .................... res = 1.0/res;
900 .................... return(res); 920 .................... return(res);
901 .................... } 921 .................... }
902 .................... 922 ....................
903 .................... 923 ....................
904 .................... //Overloaded function for exp() for PCD 924 .................... //Overloaded function for exp() for PCD
905 .................... // Overloaded function exp() for data type - Float48 925 .................... // Overloaded function exp() for data type - Float48
906 .................... #if defined(__PCD__) 926 .................... #if defined(__PCD__)
907 .................... float48 exp(float48 x) 927 .................... float48 exp(float48 x)
908 .................... { 928 .................... {
909 .................... float48 y, res, r; 929 .................... float48 y, res, r;
910 .................... int8 data1; 930 .................... int8 data1;
911 .................... signed int8 n; 931 .................... signed int8 n;
912 .................... int1 s; 932 .................... int1 s;
913 .................... #ifdef _ERRNO 933 .................... #ifdef _ERRNO
914 .................... if(x > 88.722838) 934 .................... if(x > 88.722838)
915 .................... { 935 .................... {
916 .................... errno=ERANGE; 936 .................... errno=ERANGE;
917 .................... return(0); 937 .................... return(0);
918 .................... } 938 .................... }
919 .................... #endif 939 .................... #endif
920 .................... n = (signed int16)(x/LN2); 940 .................... n = (signed int16)(x/LN2);
921 .................... s = 0; 941 .................... s = 0;
922 .................... y = x; 942 .................... y = x;
923 .................... 943 ....................
924 .................... if (x < 0) 944 .................... if (x < 0)
925 .................... { 945 .................... {
926 .................... s = 1; 946 .................... s = 1;
927 .................... n = -n; 947 .................... n = -n;
928 .................... y = -y; 948 .................... y = -y;
929 .................... } 949 .................... }
930 .................... 950 ....................
931 .................... res = 0.0; 951 .................... res = 0.0;
932 .................... 952 ....................
933 .................... data1 = n+0x7F; 953 .................... data1 = n+0x7F;
934 .................... if(bit_test(data1,0)) 954 .................... if(bit_test(data1,0))
935 .................... bit_set(*(((unsigned int8 *)(&res)+4)),7); 955 .................... bit_set(*(((unsigned int8 *)(&res)+4)),7);
936 .................... rotate_right(&data1,1); 956 .................... rotate_right(&data1,1);
937 .................... bit_clear(data1,7); 957 .................... bit_clear(data1,7);
938 .................... *(((unsigned int8 *)(&res)+5)) = data1; 958 .................... *(((unsigned int8 *)(&res)+5)) = data1;
939 .................... 959 ....................
940 .................... y = y/LN2 - (float48)n; 960 .................... y = y/LN2 - (float48)n;
941 .................... 961 ....................
942 .................... r = pe[0]*y + pe[1]; 962 .................... r = pe[0]*y + pe[1];
943 .................... r = r*y + pe[2]; 963 .................... r = r*y + pe[2];
944 .................... r = r*y + pe[3]; 964 .................... r = r*y + pe[3];
945 .................... r = r*y + pe[4]; 965 .................... r = r*y + pe[4];
946 .................... r = r*y + pe[5]; 966 .................... r = r*y + pe[5];
947 .................... 967 ....................
948 .................... res = res*(1.0 + y*r); 968 .................... res = res*(1.0 + y*r);
949 .................... 969 ....................
950 .................... if (s) 970 .................... if (s)
951 .................... res = 1.0/res; 971 .................... res = 1.0/res;
952 .................... return(res); 972 .................... return(res);
953 .................... } 973 .................... }
954 .................... 974 ....................
955 .................... // Overloaded function exp() for data type - Float64 975 .................... // Overloaded function exp() for data type - Float64
956 .................... float64 exp(float64 x) 976 .................... float64 exp(float64 x)
957 .................... { 977 .................... {
958 .................... float64 y, res, r; 978 .................... float64 y, res, r;
959 .................... unsigned int16 data1, data2; 979 .................... unsigned int16 data1, data2;
960 .................... unsigned int16 *p; 980 .................... unsigned int16 *p;
961 .................... signed int16 n; 981 .................... signed int16 n;
962 .................... int1 s; 982 .................... int1 s;
963 .................... #ifdef _ERRNO 983 .................... #ifdef _ERRNO
964 .................... if(x > 709.7827128) 984 .................... if(x > 709.7827128)
965 .................... { 985 .................... {
966 .................... errno=ERANGE; 986 .................... errno=ERANGE;
967 .................... return(0); 987 .................... return(0);
968 .................... } 988 .................... }
969 .................... #endif 989 .................... #endif
970 .................... n = (signed int16)(x/LN2); 990 .................... n = (signed int16)(x/LN2);
971 .................... s = 0; 991 .................... s = 0;
972 .................... y = x; 992 .................... y = x;
973 .................... 993 ....................
974 .................... if (x < 0) 994 .................... if (x < 0)
975 .................... { 995 .................... {
976 .................... s = 1; 996 .................... s = 1;
977 .................... n = -n; 997 .................... n = -n;
978 .................... y = -y; 998 .................... y = -y;
979 .................... } 999 .................... }
980 .................... 1000 ....................
981 .................... res = 0.0; 1001 .................... res = 0.0;
982 .................... 1002 ....................
983 .................... #if !defined(__PCD__) 1003 .................... #if !defined(__PCD__)
984 .................... *((unsigned int16 *)(&res)) = n + 0x7F; 1004 .................... *((unsigned int16 *)(&res)) = n + 0x7F;
985 .................... #endif 1005 .................... #endif
986 .................... p= (((unsigned int16 *)(&res))+3); 1006 .................... p= (((unsigned int16 *)(&res))+3);
987 .................... data1 = *p; 1007 .................... data1 = *p;
988 .................... data2 = *p; 1008 .................... data2 = *p;
989 .................... data1 = n + 0x3FF; 1009 .................... data1 = n + 0x3FF;
990 .................... data1 = data1 <<4; 1010 .................... data1 = data1 <<4;
991 .................... if(bit_test(data2,15)) 1011 .................... if(bit_test(data2,15))
992 .................... bit_set(data1,15); 1012 .................... bit_set(data1,15);
993 .................... data2 = data2 & 0x000F; 1013 .................... data2 = data2 & 0x000F;
994 .................... data1 ^= data2; 1014 .................... data1 ^= data2;
995 .................... 1015 ....................
996 .................... *(((unsigned int16 *)(&res)+3)) = data1; 1016 .................... *(((unsigned int16 *)(&res)+3)) = data1;
997 .................... 1017 ....................
998 .................... 1018 ....................
999 .................... y = y/LN2 - (float64)n; 1019 .................... y = y/LN2 - (float64)n;
1000 .................... 1020 ....................
1001 .................... r = pe[0]*y + pe[1]; 1021 .................... r = pe[0]*y + pe[1];
1002 .................... r = r*y + pe[2]; 1022 .................... r = r*y + pe[2];
1003 .................... r = r*y + pe[3]; 1023 .................... r = r*y + pe[3];
1004 .................... r = r*y + pe[4]; 1024 .................... r = r*y + pe[4];
1005 .................... r = r*y + pe[5]; 1025 .................... r = r*y + pe[5];
1006 .................... 1026 ....................
1007 .................... res = res*(1.0 + y*r); 1027 .................... res = res*(1.0 + y*r);
1008 .................... 1028 ....................
1009 .................... if (s) 1029 .................... if (s)
1010 .................... res = 1.0/res; 1030 .................... res = 1.0/res;
1011 .................... return(res); 1031 .................... return(res);
1012 .................... } 1032 .................... }
1013 .................... 1033 ....................
1014 .................... #ENDIF 1034 .................... #ENDIF
1015 .................... 1035 ....................
1016 .................... 1036 ....................
1017 .................... /************************************************************/ 1037 .................... /************************************************************/
1018 .................... 1038 ....................
1019 .................... float32 const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 1039 .................... 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}; 1040 .................... float32 const ql[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943};
1021 .................... 1041 ....................
1022 .................... //////////////////////////////////////////////////////////////////////////// 1042 .................... ////////////////////////////////////////////////////////////////////////////
1023 .................... // float log(float x) 1043 .................... // float log(float x)
1024 .................... //////////////////////////////////////////////////////////////////////////// 1044 .................... ////////////////////////////////////////////////////////////////////////////
1025 .................... // Description : returns the the natural log of x 1045 .................... // Description : returns the the natural log of x
1026 .................... // Date : N/A 1046 .................... // Date : N/A
1027 .................... // 1047 .................... //
1028 .................... float32 log(float32 x) 1048 .................... float32 log(float32 x)
1029 .................... { 1049 .................... {
1030 .................... float32 y, res, r, y2; 1050 .................... float32 y, res, r, y2;
1031 .................... #if defined(__PCD__) 1051 .................... #if defined(__PCD__)
1032 .................... unsigned int8 data1,data2; 1052 .................... unsigned int8 data1,data2;
1033 .................... #endif 1053 .................... #endif
1034 .................... signed int8 n; 1054 .................... signed int8 n;
1035 .................... #ifdef _ERRNO 1055 .................... #ifdef _ERRNO
1036 .................... if(x <0) 1056 .................... if(x <0)
1037 .................... { 1057 .................... {
1038 .................... errno=EDOM; 1058 .................... errno=EDOM;
1039 .................... } 1059 .................... }
1040 .................... if(x ==0) 1060 .................... if(x ==0)
1041 .................... { 1061 .................... {
1042 .................... errno=ERANGE; 1062 .................... errno=ERANGE;
1043 .................... return(0); 1063 .................... return(0);
1044 .................... } 1064 .................... }
1045 .................... #endif 1065 .................... #endif
1046 .................... y = x; 1066 .................... y = x;
1047 .................... 1067 ....................
1048 .................... if (y != 1.0) 1068 .................... if (y != 1.0)
1049 .................... { 1069 .................... {
1050 .................... #if !defined(__PCD__) 1070 .................... #if !defined(__PCD__)
1051 .................... *((unsigned int8 *)(&y)) = 0x7E; 1071 .................... *((unsigned int8 *)(&y)) = 0x7E;
1052 .................... #endif 1072 .................... #endif
1053 .................... 1073 ....................
1054 .................... #if defined(__PCD__) // Takes care of IEEE format 1074 .................... #if defined(__PCD__) // Takes care of IEEE format
1055 .................... data2 = *(((unsigned int8 *)(&y))+3); 1075 .................... data2 = *(((unsigned int8 *)(&y))+3);
1056 .................... *(((unsigned int8 *)(&y))+3) = 0x3F; 1076 .................... *(((unsigned int8 *)(&y))+3) = 0x3F;
1057 .................... data1 = *(((unsigned int8 *)(&y))+2); 1077 .................... data1 = *(((unsigned int8 *)(&y))+2);
1058 .................... bit_clear(data1,7); 1078 .................... bit_clear(data1,7);
1059 .................... *(((unsigned int8 *)(&y))+2) = data1; 1079 .................... *(((unsigned int8 *)(&y))+2) = data1;
1060 .................... if(bit_test(data2,7)) 1080 .................... if(bit_test(data2,7))
1061 .................... bit_set(*(((unsigned int8 *)(&y))+3),7); 1081 .................... bit_set(*(((unsigned int8 *)(&y))+3),7);
1062 .................... #endif 1082 .................... #endif
1063 .................... 1083 ....................
1064 .................... y = (y - 1.0)/(y + 1.0); 1084 .................... y = (y - 1.0)/(y + 1.0);
1065 .................... 1085 ....................
1066 .................... y2=y*y; 1086 .................... y2=y*y;
1067 .................... 1087 ....................
1068 .................... res = pl[0]*y2 + pl[1]; 1088 .................... res = pl[0]*y2 + pl[1];
1069 .................... res = res*y2 + pl[2]; 1089 .................... res = res*y2 + pl[2];
1070 .................... res = res*y2 + pl[3]; 1090 .................... res = res*y2 + pl[3];
1071 .................... 1091 ....................
1072 .................... r = ql[0]*y2 + ql[1]; 1092 .................... r = ql[0]*y2 + ql[1];
1073 .................... r = r*y2 + ql[2]; 1093 .................... r = r*y2 + ql[2];
1074 .................... r = r*y2 + ql[3]; 1094 .................... r = r*y2 + ql[3];
1075 .................... 1095 ....................
1076 .................... res = y*res/r; 1096 .................... res = y*res/r;
1077 .................... #if !defined(__PCD__) 1097 .................... #if !defined(__PCD__)
1078 .................... n = *((unsigned int8 *)(&x)) - 0x7E; 1098 .................... n = *((unsigned int8 *)(&x)) - 0x7E;
1079 .................... #endif 1099 .................... #endif
1080 .................... #if defined(__PCD__) 1100 .................... #if defined(__PCD__)
1081 .................... data1 = *(((unsigned int8 *)(&x)+3)); 1101 .................... data1 = *(((unsigned int8 *)(&x)+3));
1082 .................... rotate_left(&data1,1); 1102 .................... rotate_left(&data1,1);
1083 .................... data2 = *(((unsigned int8 *)(&x)+2)); 1103 .................... data2 = *(((unsigned int8 *)(&x)+2));
1084 .................... if(bit_test (data2,7)) 1104 .................... if(bit_test (data2,7))
1085 .................... bit_set(data1,0); 1105 .................... bit_set(data1,0);
1086 .................... n = data1 - 0x7E; 1106 .................... n = data1 - 0x7E;
1087 .................... #endif 1107 .................... #endif
1088 .................... 1108 ....................
1089 .................... if (n<0) 1109 .................... if (n<0)
1090 .................... r = -(float32)-n; 1110 .................... r = -(float32)-n;
1091 .................... else 1111 .................... else
1092 .................... r = (float32)n; 1112 .................... r = (float32)n;
1093 .................... 1113 ....................
1094 .................... res += r*LN2; 1114 .................... res += r*LN2;
1095 .................... } 1115 .................... }
1096 .................... 1116 ....................
1097 .................... else 1117 .................... else
1098 .................... res = 0.0; 1118 .................... res = 0.0;
1099 .................... 1119 ....................
1100 .................... return(res); 1120 .................... return(res);
1101 .................... } 1121 .................... }
1102 .................... 1122 ....................
1103 .................... //Overloaded function for log() for PCD 1123 .................... //Overloaded function for log() for PCD
1104 .................... // Overloaded function log() for data type - Float48 1124 .................... // Overloaded function log() for data type - Float48
1105 .................... #if defined(__PCD__) 1125 .................... #if defined(__PCD__)
1106 .................... float48 log(float48 x) 1126 .................... float48 log(float48 x)
1107 .................... { 1127 .................... {
1108 .................... float48 y, res, r, y2; 1128 .................... float48 y, res, r, y2;
1109 .................... unsigned int8 data1,data2; 1129 .................... unsigned int8 data1,data2;
1110 .................... signed int8 n; 1130 .................... signed int8 n;
1111 .................... #ifdef _ERRNO 1131 .................... #ifdef _ERRNO
1112 .................... if(x <0) 1132 .................... if(x <0)
1113 .................... { 1133 .................... {
1114 .................... errno=EDOM; 1134 .................... errno=EDOM;
1115 .................... } 1135 .................... }
1116 .................... if(x ==0) 1136 .................... if(x ==0)
1117 .................... { 1137 .................... {
1118 .................... errno=ERANGE; 1138 .................... errno=ERANGE;
1119 .................... return(0); 1139 .................... return(0);
1120 .................... } 1140 .................... }
1121 .................... #endif 1141 .................... #endif
1122 .................... y = x; 1142 .................... y = x;
1123 .................... 1143 ....................
1124 .................... if (y != 1.0) 1144 .................... if (y != 1.0)
1125 .................... { 1145 .................... {
1126 .................... 1146 ....................
1127 .................... #if !defined(__PCD__) 1147 .................... #if !defined(__PCD__)
1128 .................... *((unsigned int8 *)(&y)) = 0x7E; 1148 .................... *((unsigned int8 *)(&y)) = 0x7E;
1129 .................... #endif 1149 .................... #endif
1130 .................... data2 = *(((unsigned int8 *)(&y))+5); 1150 .................... data2 = *(((unsigned int8 *)(&y))+5);
1131 .................... *(((unsigned int8 *)(&y))+5) = 0x3F; 1151 .................... *(((unsigned int8 *)(&y))+5) = 0x3F;
1132 .................... data1 = *(((unsigned int8 *)(&y))+4); 1152 .................... data1 = *(((unsigned int8 *)(&y))+4);
1133 .................... bit_clear(data1,7); 1153 .................... bit_clear(data1,7);
1134 .................... *(((unsigned int8 *)(&y))+4) = data1; 1154 .................... *(((unsigned int8 *)(&y))+4) = data1;
1135 .................... 1155 ....................
1136 .................... if(bit_test(data2,7)) 1156 .................... if(bit_test(data2,7))
1137 .................... bit_set(*(((unsigned int8 *)(&y))+4),7); 1157 .................... bit_set(*(((unsigned int8 *)(&y))+4),7);
1138 .................... y = (y - 1.0)/(y + 1.0); 1158 .................... y = (y - 1.0)/(y + 1.0);
1139 .................... 1159 ....................
1140 .................... y2=y*y; 1160 .................... y2=y*y;
1141 .................... 1161 ....................
1142 .................... res = pl[0]*y2 + pl[1]; 1162 .................... res = pl[0]*y2 + pl[1];
1143 .................... res = res*y2 + pl[2]; 1163 .................... res = res*y2 + pl[2];
1144 .................... res = res*y2 + pl[3]; 1164 .................... res = res*y2 + pl[3];
1145 .................... 1165 ....................
1146 .................... r = ql[0]*y2 + ql[1]; 1166 .................... r = ql[0]*y2 + ql[1];
1147 .................... r = r*y2 + ql[2]; 1167 .................... r = r*y2 + ql[2];
1148 .................... r = r*y2 + ql[3]; 1168 .................... r = r*y2 + ql[3];
1149 .................... 1169 ....................
1150 .................... res = y*res/r; 1170 .................... res = y*res/r;
1151 .................... 1171 ....................
1152 .................... data1 = *(((unsigned int8 *)(&x)+5)); 1172 .................... data1 = *(((unsigned int8 *)(&x)+5));
1153 .................... rotate_left(&data1,1); 1173 .................... rotate_left(&data1,1);
1154 .................... data2 = *(((unsigned int8 *)(&x)+4)); 1174 .................... data2 = *(((unsigned int8 *)(&x)+4));
1155 .................... if(bit_test (data2,7)) 1175 .................... if(bit_test (data2,7))
1156 .................... bit_set(data1,0); 1176 .................... bit_set(data1,0);
1157 .................... 1177 ....................
1158 .................... n = data1 - 0x7E; 1178 .................... n = data1 - 0x7E;
1159 .................... 1179 ....................
1160 .................... if (n<0) 1180 .................... if (n<0)
1161 .................... r = -(float48)-n; 1181 .................... r = -(float48)-n;
1162 .................... else 1182 .................... else
1163 .................... r = (float48)n; 1183 .................... r = (float48)n;
1164 .................... 1184 ....................
1165 .................... res += r*LN2; 1185 .................... res += r*LN2;
1166 .................... } 1186 .................... }
1167 .................... 1187 ....................
1168 .................... else 1188 .................... else
1169 .................... res = 0.0; 1189 .................... res = 0.0;
1170 .................... 1190 ....................
1171 .................... return(res); 1191 .................... return(res);
1172 .................... } 1192 .................... }
1173 .................... 1193 ....................
1174 .................... // Overloaded function log() for data type - Float48 1194 .................... // Overloaded function log() for data type - Float48
1175 .................... #if defined(__PCD__) 1195 .................... #if defined(__PCD__)
1176 .................... float32 const pl_64[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 1196 .................... 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}; 1197 .................... float32 const ql_64[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943};
1178 .................... #endif 1198 .................... #endif
1179 .................... float64 log(float64 x) 1199 .................... float64 log(float64 x)
1180 .................... { 1200 .................... {
1181 .................... float64 y, res, r, y2; 1201 .................... float64 y, res, r, y2;
1182 .................... unsigned int16 data1,data2; 1202 .................... unsigned int16 data1,data2;
1183 .................... unsigned int16 *p; 1203 .................... unsigned int16 *p;
1184 .................... signed int16 n; 1204 .................... signed int16 n;
1185 .................... #ifdef _ERRNO 1205 .................... #ifdef _ERRNO
1186 .................... if(x <0) 1206 .................... if(x <0)
1187 .................... { 1207 .................... {
1188 .................... errno=EDOM; 1208 .................... errno=EDOM;
1189 .................... } 1209 .................... }
1190 .................... if(x ==0) 1210 .................... if(x ==0)
1191 .................... { 1211 .................... {
1192 .................... errno=ERANGE; 1212 .................... errno=ERANGE;
1193 .................... return(0); 1213 .................... return(0);
1194 .................... } 1214 .................... }
1195 .................... #endif 1215 .................... #endif
1196 .................... y = x; 1216 .................... y = x;
1197 .................... 1217 ....................
1198 .................... if (y != 1.0) 1218 .................... if (y != 1.0)
1199 .................... { 1219 .................... {
1200 .................... #if !defined(__PCD__) 1220 .................... #if !defined(__PCD__)
1201 .................... *((unsigned int8 *)(&y)) = 0x7E; 1221 .................... *((unsigned int8 *)(&y)) = 0x7E;
1202 .................... #endif 1222 .................... #endif
1203 .................... p= (((unsigned int16 *)(&y))+3); 1223 .................... p= (((unsigned int16 *)(&y))+3);
1204 .................... data1 = *p; 1224 .................... data1 = *p;
1205 .................... data2 = *p; 1225 .................... data2 = *p;
1206 .................... data1 = 0x3FE; 1226 .................... data1 = 0x3FE;
1207 .................... data1 = data1 <<4; 1227 .................... data1 = data1 <<4;
1208 .................... if(bit_test (data2,15)) 1228 .................... if(bit_test (data2,15))
1209 .................... bit_set(data1,15); 1229 .................... bit_set(data1,15);
1210 .................... data2 = data2 & 0x000F; 1230 .................... data2 = data2 & 0x000F;
1211 .................... data1 ^=data2; 1231 .................... data1 ^=data2;
1212 .................... 1232 ....................
1213 .................... *p = data1; 1233 .................... *p = data1;
1214 .................... 1234 ....................
1215 .................... y = (y - 1.0)/(y + 1.0); 1235 .................... y = (y - 1.0)/(y + 1.0);
1216 .................... 1236 ....................
1217 .................... y2=y*y; 1237 .................... y2=y*y;
1218 .................... 1238 ....................
1219 .................... res = pl_64[0]*y2 + pl_64[1]; 1239 .................... res = pl_64[0]*y2 + pl_64[1];
1220 .................... res = res*y2 + pl_64[2]; 1240 .................... res = res*y2 + pl_64[2];
1221 .................... res = res*y2 + pl_64[3]; 1241 .................... res = res*y2 + pl_64[3];
1222 .................... 1242 ....................
1223 .................... r = ql_64[0]*y2 + ql_64[1]; 1243 .................... r = ql_64[0]*y2 + ql_64[1];
1224 .................... r = r*y2 + ql_64[2]; 1244 .................... r = r*y2 + ql_64[2];
1225 .................... r = r*y2 + ql_64[3]; 1245 .................... r = r*y2 + ql_64[3];
1226 .................... 1246 ....................
1227 .................... res = y*res/r; 1247 .................... res = y*res/r;
1228 .................... 1248 ....................
1229 .................... p= (((unsigned int16 *)(&x))+3); 1249 .................... p= (((unsigned int16 *)(&x))+3);
1230 .................... data1 = *p; 1250 .................... data1 = *p;
1231 .................... bit_clear(data1,15); 1251 .................... bit_clear(data1,15);
1232 .................... data1 = data1 >>4; 1252 .................... data1 = data1 >>4;
1233 .................... n = data1 - 0x3FE; 1253 .................... n = data1 - 0x3FE;
1234 .................... 1254 ....................
1235 .................... 1255 ....................
1236 .................... if (n<0) 1256 .................... if (n<0)
1237 .................... r = -(float64)-n; 1257 .................... r = -(float64)-n;
1238 .................... else 1258 .................... else
1239 .................... r = (float64)n; 1259 .................... r = (float64)n;
1240 .................... 1260 ....................
1241 .................... res += r*LN2; 1261 .................... res += r*LN2;
1242 .................... } 1262 .................... }
1243 .................... 1263 ....................
1244 .................... else 1264 .................... else
1245 .................... res = 0.0; 1265 .................... res = 0.0;
1246 .................... 1266 ....................
1247 .................... return(res); 1267 .................... return(res);
1248 .................... } 1268 .................... }
1249 .................... #endif 1269 .................... #endif
1250 .................... 1270 ....................
1251 .................... 1271 ....................
1252 .................... #define LN10 2.3025850929940456 1272 .................... #define LN10 2.3025850929940456
1253 .................... 1273 ....................
1254 .................... //////////////////////////////////////////////////////////////////////////// 1274 .................... ////////////////////////////////////////////////////////////////////////////
1255 .................... // float log10(float x) 1275 .................... // float log10(float x)
1256 .................... //////////////////////////////////////////////////////////////////////////// 1276 .................... ////////////////////////////////////////////////////////////////////////////
1257 .................... // Description : returns the the log base 10 of x 1277 .................... // Description : returns the the log base 10 of x
1258 .................... // Date : N/A 1278 .................... // Date : N/A
1259 .................... // 1279 .................... //
1260 .................... float32 log10(float32 x) 1280 .................... float32 log10(float32 x)
1261 .................... { 1281 .................... {
1262 .................... float32 r; 1282 .................... float32 r;
1263 .................... 1283 ....................
1264 .................... r = log(x); 1284 .................... r = log(x);
1265 .................... r = r/LN10; 1285 .................... r = r/LN10;
1266 .................... return(r); 1286 .................... return(r);
1267 .................... } 1287 .................... }
1268 .................... 1288 ....................
1269 .................... //Overloaded functions for log10() for PCD 1289 .................... //Overloaded functions for log10() for PCD
1270 .................... // Overloaded function log10() for data type - Float48 1290 .................... // Overloaded function log10() for data type - Float48
1271 .................... #if defined(__PCD__) 1291 .................... #if defined(__PCD__)
1272 .................... float48 log10(float48 x) 1292 .................... float48 log10(float48 x)
1273 .................... { 1293 .................... {
1274 .................... float48 r; 1294 .................... float48 r;
1275 .................... 1295 ....................
1276 .................... r = log(x); 1296 .................... r = log(x);
1277 .................... r = r/LN10; 1297 .................... r = r/LN10;
1278 .................... return(r); 1298 .................... return(r);
1279 .................... } 1299 .................... }
1280 .................... 1300 ....................
1281 .................... // Overloaded function log10() for data type - Float64 1301 .................... // Overloaded function log10() for data type - Float64
1282 .................... float64 log10(float64 x) 1302 .................... float64 log10(float64 x)
1283 .................... { 1303 .................... {
1284 .................... float64 r; 1304 .................... float64 r;
1285 .................... 1305 ....................
1286 .................... r = log(x); 1306 .................... r = log(x);
1287 .................... r = r/LN10; 1307 .................... r = r/LN10;
1288 .................... return(r); 1308 .................... return(r);
1289 .................... } 1309 .................... }
1290 .................... #endif 1310 .................... #endif
1291 .................... //////////////////////////////////////////////////////////////////////////// 1311 .................... ////////////////////////////////////////////////////////////////////////////
1292 .................... // float modf(float x) 1312 .................... // float modf(float x)
1293 .................... //////////////////////////////////////////////////////////////////////////// 1313 .................... ////////////////////////////////////////////////////////////////////////////
1294 .................... // Description :breaks the argument value int integral and fractional parts, 1314 .................... // 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 1315 .................... // 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 1316 .................... // as a float in the object pointed to by the iptr
1297 .................... // Returns : returns the signed fractional part of value. 1317 .................... // Returns : returns the signed fractional part of value.
1298 .................... // Date : N/A 1318 .................... // Date : N/A
1299 .................... // 1319 .................... //
1300 .................... 1320 ....................
1301 .................... float32 modf(float32 value,float32 *iptr) 1321 .................... float32 modf(float32 value,float32 *iptr)
1302 .................... { 1322 .................... {
1303 .................... *iptr=(value < 0.0)? ceil(value): floor(value); 1323 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1304 .................... return(value - *iptr); 1324 .................... return(value - *iptr);
1305 .................... } 1325 .................... }
1306 .................... //Overloaded functions for modf() for PCD 1326 .................... //Overloaded functions for modf() for PCD
1307 .................... // Overloaded function modf() for data type - Float48 1327 .................... // Overloaded function modf() for data type - Float48
1308 .................... #if defined(__PCD__) 1328 .................... #if defined(__PCD__)
1309 .................... float48 modf(float48 value,float48 *iptr) 1329 .................... float48 modf(float48 value,float48 *iptr)
1310 .................... { 1330 .................... {
1311 .................... *iptr=(value < 0.0)? ceil(value): floor(value); 1331 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1312 .................... return(value - *iptr); 1332 .................... return(value - *iptr);
1313 .................... } 1333 .................... }
1314 .................... // Overloaded function modf() for data type - Float64 1334 .................... // Overloaded function modf() for data type - Float64
1315 .................... float64 modf(float64 value,float64 *iptr) 1335 .................... float64 modf(float64 value,float64 *iptr)
1316 .................... { 1336 .................... {
1317 .................... *iptr=(value < 0.0)? ceil(value): floor(value); 1337 .................... *iptr=(value < 0.0)? ceil(value): floor(value);
1318 .................... return(value - *iptr); 1338 .................... return(value - *iptr);
1319 .................... } 1339 .................... }
1320 .................... #endif 1340 .................... #endif
1321 .................... 1341 ....................
1322 .................... //////////////////////////////////////////////////////////////////////////// 1342 .................... ////////////////////////////////////////////////////////////////////////////
1323 .................... // float pwr(float x,float y) 1343 .................... // float pwr(float x,float y)
1324 .................... //////////////////////////////////////////////////////////////////////////// 1344 .................... ////////////////////////////////////////////////////////////////////////////
1325 .................... // Description : returns the value (x^y) 1345 .................... // Description : returns the value (x^y)
1326 .................... // Date : N/A 1346 .................... // Date : N/A
1327 .................... // Note : 0 is returned when the function will generate an imaginary number 1347 .................... // Note : 0 is returned when the function will generate an imaginary number
1328 .................... // 1348 .................... //
1329 .................... float32 pwr(float32 x,float32 y) 1349 .................... float32 pwr(float32 x,float32 y)
1330 .................... { 1350 .................... {
1331 .................... if(0 > x && fmod(y, 1) == 0) { 1351 .................... if(0 > x && fmod(y, 1) == 0) {
1332 .................... if(fmod(y, 2) == 0) { 1352 .................... if(fmod(y, 2) == 0) {
1333 .................... return (exp(log(-x) * y)); 1353 .................... return (exp(log(-x) * y));
1334 .................... } else { 1354 .................... } else {
1335 .................... return (-exp(log(-x) * y)); 1355 .................... return (-exp(log(-x) * y));
1336 .................... } 1356 .................... }
1337 .................... } else if(0 > x && fmod(y, 1) != 0) { 1357 .................... } else if(0 > x && fmod(y, 1) != 0) {
1338 .................... return 0; 1358 .................... return 0;
1339 .................... } else { 1359 .................... } else {
1340 .................... if(x != 0 || 0 >= y) { 1360 .................... if(x != 0 || 0 >= y) {
1341 .................... return (exp(log(x) * y)); 1361 .................... return (exp(log(x) * y));
1342 .................... } 1362 .................... }
1343 .................... } 1363 .................... }
1344 .................... } 1364 .................... }
1345 .................... //Overloaded functions for pwr() for PCD 1365 .................... //Overloaded functions for pwr() for PCD
1346 .................... // Overloaded function pwr() for data type - Float48 1366 .................... // Overloaded function pwr() for data type - Float48
1347 .................... #if defined(__PCD__) 1367 .................... #if defined(__PCD__)
1348 .................... float48 pwr(float48 x,float48 y) 1368 .................... float48 pwr(float48 x,float48 y)
1349 .................... { 1369 .................... {
1350 .................... if(0 > x && fmod(y, 1) == 0) { 1370 .................... if(0 > x && fmod(y, 1) == 0) {
1351 .................... if(fmod(y, 2) == 0) { 1371 .................... if(fmod(y, 2) == 0) {
1352 .................... return (exp(log(-x) * y)); 1372 .................... return (exp(log(-x) * y));
1353 .................... } else { 1373 .................... } else {
1354 .................... return (-exp(log(-x) * y)); 1374 .................... return (-exp(log(-x) * y));
1355 .................... } 1375 .................... }
1356 .................... } else if(0 > x && fmod(y, 1) != 0) { 1376 .................... } else if(0 > x && fmod(y, 1) != 0) {
1357 .................... return 0; 1377 .................... return 0;
1358 .................... } else { 1378 .................... } else {
1359 .................... if(x != 0 || 0 >= y) { 1379 .................... if(x != 0 || 0 >= y) {
1360 .................... return (exp(log(x) * y)); 1380 .................... return (exp(log(x) * y));
1361 .................... } 1381 .................... }
1362 .................... } 1382 .................... }
1363 .................... } 1383 .................... }
1364 .................... // Overloaded function pwr() for data type - Float64 1384 .................... // Overloaded function pwr() for data type - Float64
1365 .................... float64 pwr(float64 x,float64 y) 1385 .................... float64 pwr(float64 x,float64 y)
1366 .................... { 1386 .................... {
1367 .................... if(0 > x && fmod(y, 1) == 0) { 1387 .................... if(0 > x && fmod(y, 1) == 0) {
1368 .................... if(fmod(y, 2) == 0) { 1388 .................... if(fmod(y, 2) == 0) {
1369 .................... return (exp(log(-x) * y)); 1389 .................... return (exp(log(-x) * y));
1370 .................... } else { 1390 .................... } else {
1371 .................... return (-exp(log(-x) * y)); 1391 .................... return (-exp(log(-x) * y));
1372 .................... } 1392 .................... }
1373 .................... } else if(0 > x && fmod(y, 1) != 0) { 1393 .................... } else if(0 > x && fmod(y, 1) != 0) {
1374 .................... return 0; 1394 .................... return 0;
1375 .................... } else { 1395 .................... } else {
1376 .................... if(x != 0 || 0 >= y) { 1396 .................... if(x != 0 || 0 >= y) {
1377 .................... return (exp(log(x) * y)); 1397 .................... return (exp(log(x) * y));
1378 .................... } 1398 .................... }
1379 .................... } 1399 .................... }
1380 .................... } 1400 .................... }
1381 .................... #endif 1401 .................... #endif
1382 .................... 1402 ....................
1383 .................... //////////////////// Power functions //////////////////// 1403 .................... //////////////////// Power functions ////////////////////
1384 .................... 1404 ....................
1385 .................... //////////////////////////////////////////////////////////////////////////// 1405 .................... ////////////////////////////////////////////////////////////////////////////
1386 .................... // float pow(float x,float y) 1406 .................... // float pow(float x,float y)
1387 .................... //////////////////////////////////////////////////////////////////////////// 1407 .................... ////////////////////////////////////////////////////////////////////////////
1388 .................... // Description : returns the value (x^y) 1408 .................... // Description : returns the value (x^y)
1389 .................... // Date : N/A 1409 .................... // Date : N/A
1390 .................... // Note : 0 is returned when the function will generate an imaginary number 1410 .................... // Note : 0 is returned when the function will generate an imaginary number
1391 .................... // 1411 .................... //
1392 .................... float32 pow(float32 x,float32 y) 1412 .................... float32 pow(float32 x,float32 y)
1393 .................... { 1413 .................... {
1394 .................... if(0 > x && fmod(y, 1) == 0) { 1414 .................... if(0 > x && fmod(y, 1) == 0) {
1395 .................... if(fmod(y, 2) == 0) { 1415 .................... if(fmod(y, 2) == 0) {
1396 .................... return (exp(log(-x) * y)); 1416 .................... return (exp(log(-x) * y));
1397 .................... } else { 1417 .................... } else {
1398 .................... return (-exp(log(-x) * y)); 1418 .................... return (-exp(log(-x) * y));
1399 .................... } 1419 .................... }
1400 .................... } else if(0 > x && fmod(y, 1) != 0) { 1420 .................... } else if(0 > x && fmod(y, 1) != 0) {
1401 .................... return 0; 1421 .................... return 0;
1402 .................... } else { 1422 .................... } else {
1403 .................... if(x != 0 || 0 >= y) { 1423 .................... if(x != 0 || 0 >= y) {
1404 .................... return (exp(log(x) * y)); 1424 .................... return (exp(log(x) * y));
1405 .................... } 1425 .................... }
1406 .................... } 1426 .................... }
1407 .................... } 1427 .................... }
1408 .................... //Overloaded functions for pow() for PCD 1428 .................... //Overloaded functions for pow() for PCD
1409 .................... // Overloaded function for pow() data type - Float48 1429 .................... // Overloaded function for pow() data type - Float48
1410 .................... #if defined(__PCD__) 1430 .................... #if defined(__PCD__)
1411 .................... float48 pow(float48 x,float48 y) 1431 .................... float48 pow(float48 x,float48 y)
1412 .................... { 1432 .................... {
1413 .................... if(0 > x && fmod(y, 1) == 0) { 1433 .................... if(0 > x && fmod(y, 1) == 0) {
1414 .................... if(fmod(y, 2) == 0) { 1434 .................... if(fmod(y, 2) == 0) {
1415 .................... return (exp(log(-x) * y)); 1435 .................... return (exp(log(-x) * y));
1416 .................... } else { 1436 .................... } else {
1417 .................... return (-exp(log(-x) * y)); 1437 .................... return (-exp(log(-x) * y));
1418 .................... } 1438 .................... }
1419 .................... } else if(0 > x && fmod(y, 1) != 0) { 1439 .................... } else if(0 > x && fmod(y, 1) != 0) {
1420 .................... return 0; 1440 .................... return 0;
1421 .................... } else { 1441 .................... } else {
1422 .................... if(x != 0 || 0 >= y) { 1442 .................... if(x != 0 || 0 >= y) {
1423 .................... return (exp(log(x) * y)); 1443 .................... return (exp(log(x) * y));
1424 .................... } 1444 .................... }
1425 .................... } 1445 .................... }
1426 .................... } 1446 .................... }
1427 .................... 1447 ....................
1428 .................... // Overloaded function pow() for data type - Float64 1448 .................... // Overloaded function pow() for data type - Float64
1429 .................... float64 pow(float64 x,float64 y) 1449 .................... float64 pow(float64 x,float64 y)
1430 .................... { 1450 .................... {
1431 .................... if(0 > x && fmod(y, 1) == 0) { 1451 .................... if(0 > x && fmod(y, 1) == 0) {
1432 .................... if(fmod(y, 2) == 0) { 1452 .................... if(fmod(y, 2) == 0) {
1433 .................... return (exp(log(-x) * y)); 1453 .................... return (exp(log(-x) * y));
1434 .................... } else { 1454 .................... } else {
1435 .................... return (-exp(log(-x) * y)); 1455 .................... return (-exp(log(-x) * y));
1436 .................... } 1456 .................... }
1437 .................... } else if(0 > x && fmod(y, 1) != 0) { 1457 .................... } else if(0 > x && fmod(y, 1) != 0) {
1438 .................... return 0; 1458 .................... return 0;
1439 .................... } else { 1459 .................... } else {
1440 .................... if(x != 0 || 0 >= y) { 1460 .................... if(x != 0 || 0 >= y) {
1441 .................... return (exp(log(x) * y)); 1461 .................... return (exp(log(x) * y));
1442 .................... } 1462 .................... }
1443 .................... } 1463 .................... }
1444 .................... } 1464 .................... }
1445 .................... #endif 1465 .................... #endif
1446 .................... 1466 ....................
1447 .................... //////////////////////////////////////////////////////////////////////////// 1467 .................... ////////////////////////////////////////////////////////////////////////////
1448 .................... // float sqrt(float x) 1468 .................... // float sqrt(float x)
1449 .................... //////////////////////////////////////////////////////////////////////////// 1469 .................... ////////////////////////////////////////////////////////////////////////////
1450 .................... // Description : returns the square root of x 1470 .................... // Description : returns the square root of x
1451 .................... // Date : N/A 1471 .................... // Date : N/A
1452 .................... // 1472 .................... //
1453 .................... float32 sqrt(float32 x) 1473 .................... float32 sqrt(float32 x)
1454 .................... { 1474 .................... {
1455 .................... float32 y, res; 1475 .................... float32 y, res;
1456 .................... #if defined(__PCD__) 1476 .................... #if defined(__PCD__)
1457 .................... unsigned int16 data1,data2; 1477 .................... unsigned int16 data1,data2;
1458 .................... #endif 1478 .................... #endif
1459 .................... BYTE *p; 1479 .................... BYTE *p;
1460 .................... 1480 ....................
1461 .................... #ifdef _ERRNO 1481 .................... #ifdef _ERRNO
1462 .................... if(x < 0) 1482 .................... if(x < 0)
1463 .................... { 1483 .................... {
1464 .................... errno=EDOM; 1484 .................... errno=EDOM;
1465 .................... } 1485 .................... }
1466 .................... #endif 1486 .................... #endif
1467 .................... 1487 ....................
1468 .................... if( x<=0.0) 1488 .................... if( x<=0.0)
1469 .................... return(0.0); 1489 .................... return(0.0);
1470 .................... 1490 ....................
1471 .................... y=x; 1491 .................... y=x;
1472 .................... 1492 ....................
1473 .................... #if !defined(__PCD__) 1493 .................... #if !defined(__PCD__)
1474 .................... p=&y; 1494 .................... p=&y;
1475 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); 1495 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1);
1476 .................... #endif 1496 .................... #endif
1477 .................... 1497 ....................
1478 .................... #if defined(__PCD__) 1498 .................... #if defined(__PCD__)
1479 .................... p = (((unsigned int8 *)(&y))+3); 1499 .................... p = (((unsigned int8 *)(&y))+3);
1480 .................... data1 = *(((unsigned int8 *)(&y))+3); 1500 .................... data1 = *(((unsigned int8 *)(&y))+3);
1481 .................... data2 = *(((unsigned int8 *)(&y))+2); 1501 .................... data2 = *(((unsigned int8 *)(&y))+2);
1482 .................... rotate_left(&data1,1); 1502 .................... rotate_left(&data1,1);
1483 .................... if(bit_test(data2,7)) 1503 .................... if(bit_test(data2,7))
1484 .................... bit_set(data1,0); 1504 .................... bit_set(data1,0);
1485 .................... data1 = ((data1+127) >>1); 1505 .................... data1 = ((data1+127) >>1);
1486 .................... bit_clear(data2,7); 1506 .................... bit_clear(data2,7);
1487 .................... if(bit_test(data1,0)) 1507 .................... if(bit_test(data1,0))
1488 .................... bit_set(data2,7); 1508 .................... bit_set(data2,7);
1489 .................... data1 = data1 >>1; 1509 .................... data1 = data1 >>1;
1490 .................... *(((unsigned int8 *)(&y))+3) = data1; 1510 .................... *(((unsigned int8 *)(&y))+3) = data1;
1491 .................... *(((unsigned int8 *)(&y))+2) = data2; 1511 .................... *(((unsigned int8 *)(&y))+2) = data2;
1492 .................... 1512 ....................
1493 .................... #endif 1513 .................... #endif
1494 .................... 1514 ....................
1495 .................... do { 1515 .................... do {
1496 .................... res=y; 1516 .................... res=y;
1497 .................... y+=(x/y); 1517 .................... y+=(x/y);
1498 .................... 1518 ....................
1499 .................... #if !defined(__PCD__) 1519 .................... #if !defined(__PCD__)
1500 .................... (*p)--; 1520 .................... (*p)--;
1501 .................... #endif 1521 .................... #endif
1502 .................... 1522 ....................
1503 .................... #if defined(__PCD__) 1523 .................... #if defined(__PCD__)
1504 .................... data1 = *(((unsigned int8 *)(&y))+3); 1524 .................... data1 = *(((unsigned int8 *)(&y))+3);
1505 .................... data2 = *(((unsigned int8 *)(&y))+2); 1525 .................... data2 = *(((unsigned int8 *)(&y))+2);
1506 .................... rotate_left(&data1,1); 1526 .................... rotate_left(&data1,1);
1507 .................... if(bit_test(data2,7)) 1527 .................... if(bit_test(data2,7))
1508 .................... bit_set(data1,0); 1528 .................... bit_set(data1,0);
1509 .................... data1--; 1529 .................... data1--;
1510 .................... bit_clear(data2,7); 1530 .................... bit_clear(data2,7);
1511 .................... if(bit_test(data1,0)) 1531 .................... if(bit_test(data1,0))
1512 .................... bit_set(data2,7); 1532 .................... bit_set(data2,7);
1513 .................... data1 = data1 >>1; 1533 .................... data1 = data1 >>1;
1514 .................... *(((unsigned int8 *)(&y))+3) = data1; 1534 .................... *(((unsigned int8 *)(&y))+3) = data1;
1515 .................... *(((unsigned int8 *)(&y))+2) = data2; 1535 .................... *(((unsigned int8 *)(&y))+2) = data2;
1516 .................... 1536 ....................
1517 .................... #endif 1537 .................... #endif
1518 .................... } while(res != y); 1538 .................... } while(res != y);
1519 .................... 1539 ....................
1520 .................... return(res); 1540 .................... return(res);
1521 .................... } 1541 .................... }
1522 .................... //Overloaded functions for sqrt() for PCD 1542 .................... //Overloaded functions for sqrt() for PCD
1523 .................... // Overloaded function sqrt() for data type - Float48 1543 .................... // Overloaded function sqrt() for data type - Float48
1524 .................... #if defined(__PCD__) 1544 .................... #if defined(__PCD__)
1525 .................... float48 sqrt(float48 x) 1545 .................... float48 sqrt(float48 x)
1526 .................... { 1546 .................... {
1527 .................... float48 y, res; 1547 .................... float48 y, res;
1528 .................... unsigned int16 data1,data2; 1548 .................... unsigned int16 data1,data2;
1529 .................... BYTE *p; 1549 .................... BYTE *p;
1530 .................... 1550 ....................
1531 .................... #ifdef _ERRNO 1551 .................... #ifdef _ERRNO
1532 .................... if(x < 0) 1552 .................... if(x < 0)
1533 .................... { 1553 .................... {
1534 .................... errno=EDOM; 1554 .................... errno=EDOM;
1535 .................... } 1555 .................... }
1536 .................... #endif 1556 .................... #endif
1537 .................... 1557 ....................
1538 .................... if( x<=0.0) 1558 .................... if( x<=0.0)
1539 .................... return(0.0); 1559 .................... return(0.0);
1540 .................... 1560 ....................
1541 .................... y=x; 1561 .................... y=x;
1542 .................... 1562 ....................
1543 .................... #if !defined(__PCD__) 1563 .................... #if !defined(__PCD__)
1544 .................... p=&y; 1564 .................... p=&y;
1545 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); 1565 .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1);
1546 .................... #endif 1566 .................... #endif
1547 .................... 1567 ....................
1548 .................... #if defined(__PCD__) 1568 .................... #if defined(__PCD__)
1549 .................... p = (((unsigned int8 *)(&y))+5); 1569 .................... p = (((unsigned int8 *)(&y))+5);
1550 .................... data1 = *(((unsigned int8 *)(&y))+5); 1570 .................... data1 = *(((unsigned int8 *)(&y))+5);
1551 .................... data2 = *(((unsigned int8 *)(&y))+4); 1571 .................... data2 = *(((unsigned int8 *)(&y))+4);
1552 .................... rotate_left(&data1,1); 1572 .................... rotate_left(&data1,1);
1553 .................... if(bit_test(data2,7)) 1573 .................... if(bit_test(data2,7))
1554 .................... bit_set(data1,0); 1574 .................... bit_set(data1,0);
1555 .................... data1 = ((data1+127) >>1); 1575 .................... data1 = ((data1+127) >>1);
1556 .................... bit_clear(data2,7); 1576 .................... bit_clear(data2,7);
1557 .................... if(bit_test(data1,0)) 1577 .................... if(bit_test(data1,0))
1558 .................... bit_set(data2,7); 1578 .................... bit_set(data2,7);
1559 .................... data1 = data1 >>1; 1579 .................... data1 = data1 >>1;
1560 .................... *(((unsigned int8 *)(&y))+5) = data1; 1580 .................... *(((unsigned int8 *)(&y))+5) = data1;
1561 .................... *(((unsigned int8 *)(&y))+4) = data2; 1581 .................... *(((unsigned int8 *)(&y))+4) = data2;
1562 .................... 1582 ....................
1563 .................... #endif 1583 .................... #endif
1564 .................... 1584 ....................
1565 .................... do { 1585 .................... do {
1566 .................... res=y; 1586 .................... res=y;
1567 .................... y+=(x/y); 1587 .................... y+=(x/y);
1568 .................... 1588 ....................
1569 .................... #if !defined(__PCD__) 1589 .................... #if !defined(__PCD__)
1570 .................... (*p)--; 1590 .................... (*p)--;
1571 .................... #endif 1591 .................... #endif
1572 .................... 1592 ....................
1573 .................... data1 = *(((unsigned int8 *)(&y))+5); 1593 .................... data1 = *(((unsigned int8 *)(&y))+5);
1574 .................... data2 = *(((unsigned int8 *)(&y))+4); 1594 .................... data2 = *(((unsigned int8 *)(&y))+4);
1575 .................... rotate_left(&data1,1); 1595 .................... rotate_left(&data1,1);
1576 .................... if(bit_test(data2,7)) 1596 .................... if(bit_test(data2,7))
1577 .................... bit_set(data1,0); 1597 .................... bit_set(data1,0);
1578 .................... data1--; 1598 .................... data1--;
1579 .................... bit_clear(data2,7); 1599 .................... bit_clear(data2,7);
1580 .................... if(bit_test(data1,0)) 1600 .................... if(bit_test(data1,0))
1581 .................... bit_set(data2,7); 1601 .................... bit_set(data2,7);
1582 .................... data1 = data1 >>1; 1602 .................... data1 = data1 >>1;
1583 .................... *(((unsigned int8 *)(&y))+5) = data1; 1603 .................... *(((unsigned int8 *)(&y))+5) = data1;
1584 .................... *(((unsigned int8 *)(&y))+4) = data2; 1604 .................... *(((unsigned int8 *)(&y))+4) = data2;
1585 .................... 1605 ....................
1586 .................... } while(res != y); 1606 .................... } while(res != y);
1587 .................... 1607 ....................
1588 .................... return(res); 1608 .................... return(res);
1589 .................... } 1609 .................... }
1590 .................... 1610 ....................
1591 .................... // Overloaded function sqrt() for data type - Float64 1611 .................... // Overloaded function sqrt() for data type - Float64
1592 .................... float64 sqrt(float64 x) 1612 .................... float64 sqrt(float64 x)
1593 .................... { 1613 .................... {
1594 .................... float64 y, res; 1614 .................... float64 y, res;
1595 .................... unsigned int16 *p; 1615 .................... unsigned int16 *p;
1596 .................... unsigned int16 temp1,temp2; 1616 .................... unsigned int16 temp1,temp2;
1597 .................... 1617 ....................
1598 .................... #ifdef _ERRNO 1618 .................... #ifdef _ERRNO
1599 .................... if(x < 0) 1619 .................... if(x < 0)
1600 .................... { 1620 .................... {
1601 .................... errno=EDOM; 1621 .................... errno=EDOM;
1602 .................... } 1622 .................... }
1603 .................... #endif 1623 .................... #endif
1604 .................... 1624 ....................
1605 .................... if( x<=0.0) 1625 .................... if( x<=0.0)
1606 .................... return(0.0); 1626 .................... return(0.0);
1607 .................... 1627 ....................
1608 .................... y=x; 1628 .................... y=x;
1609 .................... p= (((unsigned int16 *)(&y))+3); 1629 .................... p= (((unsigned int16 *)(&y))+3);
1610 .................... temp1 = *p; 1630 .................... temp1 = *p;
1611 .................... temp2 = *p; 1631 .................... temp2 = *p;
1612 .................... bit_clear(temp1,15); 1632 .................... bit_clear(temp1,15);
1613 .................... temp1 = (temp1>>4)+1023; 1633 .................... temp1 = (temp1>>4)+1023;
1614 .................... temp1 = temp1 >> 1; 1634 .................... temp1 = temp1 >> 1;
1615 .................... temp1 = (temp1<<4) & 0xFFF0; 1635 .................... temp1 = (temp1<<4) & 0xFFF0;
1616 .................... if(bit_test(temp2,15)) 1636 .................... if(bit_test(temp2,15))
1617 .................... bit_set(temp1,15); 1637 .................... bit_set(temp1,15);
1618 .................... temp2 = temp2 & 0x000F; 1638 .................... temp2 = temp2 & 0x000F;
1619 .................... temp1 ^= temp2; 1639 .................... temp1 ^= temp2;
1620 .................... 1640 ....................
1621 .................... (*p) = temp1; 1641 .................... (*p) = temp1;
1622 .................... 1642 ....................
1623 .................... do { 1643 .................... do {
1624 .................... res=y; 1644 .................... res=y;
1625 .................... y+=(x/y); 1645 .................... y+=(x/y);
1626 .................... temp1 = *p; 1646 .................... temp1 = *p;
1627 .................... temp2 = *p; 1647 .................... temp2 = *p;
1628 .................... bit_clear(temp1,15); 1648 .................... bit_clear(temp1,15);
1629 .................... temp1 = (temp1>>4); 1649 .................... temp1 = (temp1>>4);
1630 .................... temp1--; 1650 .................... temp1--;
1631 .................... temp1 = (temp1<<4) & 0xFFF0; 1651 .................... temp1 = (temp1<<4) & 0xFFF0;
1632 .................... if(bit_test(temp2,15)) 1652 .................... if(bit_test(temp2,15))
1633 .................... bit_set(temp1,15); 1653 .................... bit_set(temp1,15);
1634 .................... temp2 = temp2 & 0x000F; 1654 .................... temp2 = temp2 & 0x000F;
1635 .................... temp1 ^= temp2; 1655 .................... temp1 ^= temp2;
1636 .................... (*p) = temp1; 1656 .................... (*p) = temp1;
1637 .................... 1657 ....................
1638 .................... } while(res != y); 1658 .................... } while(res != y);
1639 .................... 1659 ....................
1640 .................... return(res); 1660 .................... return(res);
1641 .................... } 1661 .................... }
1642 .................... #endif 1662 .................... #endif
1643 .................... 1663 ....................
1644 .................... ////////////////////////////// Trig Functions ////////////////////////////// 1664 .................... ////////////////////////////// Trig Functions //////////////////////////////
1645 .................... #ifdef PI_DIV_BY_TWO 1665 .................... #ifdef PI_DIV_BY_TWO
1646 .................... #undef PI_DIV_BY_TWO 1666 .................... #undef PI_DIV_BY_TWO
1647 .................... #endif 1667 .................... #endif
1648 .................... #define PI_DIV_BY_TWO 1.5707963267948966 1668 .................... #define PI_DIV_BY_TWO 1.5707963267948966
1649 .................... #ifdef TWOBYPI 1669 .................... #ifdef TWOBYPI
1650 .................... #undef TWOBYPI 1670 .................... #undef TWOBYPI
1651 .................... #define TWOBYPI 0.6366197723675813 1671 .................... #define TWOBYPI 0.6366197723675813
1652 .................... #endif 1672 .................... #endif
1653 .................... //////////////////////////////////////////////////////////////////////////// 1673 .................... ////////////////////////////////////////////////////////////////////////////
1654 .................... // float cos(float x) 1674 .................... // float cos(float x)
1655 .................... //////////////////////////////////////////////////////////////////////////// 1675 .................... ////////////////////////////////////////////////////////////////////////////
1656 .................... // Description : returns the cosine value of the angle x, which is in radian 1676 .................... // Description : returns the cosine value of the angle x, which is in radian
1657 .................... // Date : 9/20/2001 1677 .................... // Date : 9/20/2001
1658 .................... // 1678 .................... //
1659 .................... float32 cos(float32 x) 1679 .................... float32 cos(float32 x)
1660 .................... { 1680 .................... {
1661 .................... float32 y, t, t2 = 1.0; 1681 .................... float32 y, t, t2 = 1.0;
1662 .................... unsigned int8 quad, i; 1682 .................... unsigned int8 quad, i;
1663 .................... float32 frac; 1683 .................... float32 frac;
1664 .................... float32 p[6] = { //by the series definition for cosine 1684 .................... float32 p[6] = { //by the series definition for cosine
1665 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) 1685 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1666 .................... 0.04166666666667, 1686 .................... 0.04166666666667,
1667 .................... -0.00138888888889, 1687 .................... -0.00138888888889,
1668 .................... 0.00002480158730, 1688 .................... 0.00002480158730,
1669 .................... -0.00000027557319, 1689 .................... -0.00000027557319,
1670 .................... 0.00000000208767, 1690 .................... 0.00000000208767,
1671 .................... //-0.00000000001147, 1691 .................... //-0.00000000001147,
1672 .................... // 0.00000000000005 1692 .................... // 0.00000000000005
1673 .................... }; 1693 .................... };
1674 .................... 1694 ....................
1675 .................... if (x < 0) x = -x; // absolute value of input 1695 .................... if (x < 0) x = -x; // absolute value of input
1676 .................... 1696 ....................
1677 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant 1697 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1678 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 1698 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1679 .................... quad = quad % 4; // quadrant (0 to 3) 1699 .................... quad = quad % 4; // quadrant (0 to 3)
1680 .................... 1700 ....................
1681 .................... if (quad == 0 || quad == 2) 1701 .................... if (quad == 0 || quad == 2)
1682 .................... t = frac * PI_DIV_BY_TWO; 1702 .................... t = frac * PI_DIV_BY_TWO;
1683 .................... else if (quad == 1) 1703 .................... else if (quad == 1)
1684 .................... t = (1-frac) * PI_DIV_BY_TWO; 1704 .................... t = (1-frac) * PI_DIV_BY_TWO;
1685 .................... else // should be 3 1705 .................... else // should be 3
1686 .................... t = (frac-1) * PI_DIV_BY_TWO; 1706 .................... t = (frac-1) * PI_DIV_BY_TWO;
1687 .................... 1707 ....................
1688 .................... y = 1.0; 1708 .................... y = 1.0;
1689 .................... t = t * t; 1709 .................... t = t * t;
1690 .................... for (i = 0; i <= 5; i++) 1710 .................... for (i = 0; i <= 5; i++)
1691 .................... { 1711 .................... {
1692 .................... t2 = t2 * t; 1712 .................... t2 = t2 * t;
1693 .................... y = y + p[i] * t2; 1713 .................... y = y + p[i] * t2;
1694 .................... } 1714 .................... }
1695 .................... 1715 ....................
1696 .................... if (quad == 2 || quad == 1) 1716 .................... if (quad == 2 || quad == 1)
1697 .................... y = -y; // correct sign 1717 .................... y = -y; // correct sign
1698 .................... 1718 ....................
1699 .................... return (y); 1719 .................... return (y);
1700 .................... } 1720 .................... }
1701 .................... 1721 ....................
1702 .................... 1722 ....................
1703 .................... //Overloaded functions for cos() for PCD 1723 .................... //Overloaded functions for cos() for PCD
1704 .................... // Overloaded function cos() for data type - Float48 1724 .................... // Overloaded function cos() for data type - Float48
1705 .................... #if defined(__PCD__) 1725 .................... #if defined(__PCD__)
1706 .................... float48 cos(float48 x) 1726 .................... float48 cos(float48 x)
1707 .................... { 1727 .................... {
1708 .................... float48 y, t, t2 = 1.0; 1728 .................... float48 y, t, t2 = 1.0;
1709 .................... unsigned int8 quad, i; 1729 .................... unsigned int8 quad, i;
1710 .................... float48 frac; 1730 .................... float48 frac;
1711 .................... float48 p[6] = { //by the series definition for cosine 1731 .................... float48 p[6] = { //by the series definition for cosine
1712 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) 1732 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1713 .................... 0.04166666666667, 1733 .................... 0.04166666666667,
1714 .................... -0.00138888888889, 1734 .................... -0.00138888888889,
1715 .................... 0.00002480158730, 1735 .................... 0.00002480158730,
1716 .................... -0.00000027557319, 1736 .................... -0.00000027557319,
1717 .................... 0.00000000208767, 1737 .................... 0.00000000208767,
1718 .................... //-0.00000000001147, 1738 .................... //-0.00000000001147,
1719 .................... // 0.00000000000005 1739 .................... // 0.00000000000005
1720 .................... }; 1740 .................... };
1721 .................... 1741 ....................
1722 .................... if (x < 0) x = -x; // absolute value of input 1742 .................... if (x < 0) x = -x; // absolute value of input
1723 .................... 1743 ....................
1724 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant 1744 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1725 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 1745 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1726 .................... quad = quad % 4; // quadrant (0 to 3) 1746 .................... quad = quad % 4; // quadrant (0 to 3)
1727 .................... 1747 ....................
1728 .................... if (quad == 0 || quad == 2) 1748 .................... if (quad == 0 || quad == 2)
1729 .................... t = frac * PI_DIV_BY_TWO; 1749 .................... t = frac * PI_DIV_BY_TWO;
1730 .................... else if (quad == 1) 1750 .................... else if (quad == 1)
1731 .................... t = (1-frac) * PI_DIV_BY_TWO; 1751 .................... t = (1-frac) * PI_DIV_BY_TWO;
1732 .................... else // should be 3 1752 .................... else // should be 3
1733 .................... t = (frac-1) * PI_DIV_BY_TWO; 1753 .................... t = (frac-1) * PI_DIV_BY_TWO;
1734 .................... 1754 ....................
1735 .................... y = 0.999999999781; 1755 .................... y = 0.999999999781;
1736 .................... t = t * t; 1756 .................... t = t * t;
1737 .................... for (i = 0; i <= 5; i++) 1757 .................... for (i = 0; i <= 5; i++)
1738 .................... { 1758 .................... {
1739 .................... t2 = t2 * t; 1759 .................... t2 = t2 * t;
1740 .................... y = y + p[i] * t2; 1760 .................... y = y + p[i] * t2;
1741 .................... } 1761 .................... }
1742 .................... 1762 ....................
1743 .................... if (quad == 2 || quad == 1) 1763 .................... if (quad == 2 || quad == 1)
1744 .................... y = -y; // correct sign 1764 .................... y = -y; // correct sign
1745 .................... 1765 ....................
1746 .................... return (y); 1766 .................... return (y);
1747 .................... } 1767 .................... }
1748 .................... 1768 ....................
1749 .................... // Overloaded function cos() for data type - Float48 1769 .................... // Overloaded function cos() for data type - Float48
1750 .................... float64 cos(float64 x) 1770 .................... float64 cos(float64 x)
1751 .................... { 1771 .................... {
1752 .................... float64 y, t, t2 = 1.0; 1772 .................... float64 y, t, t2 = 1.0;
1753 .................... unsigned int8 quad, i; 1773 .................... unsigned int8 quad, i;
1754 .................... float64 frac; 1774 .................... float64 frac;
1755 .................... float64 p[6] = { //by the series definition for cosine 1775 .................... float64 p[6] = { //by the series definition for cosine
1756 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! ) 1776 .................... -0.5, // sum ( ( (-1)^n * x^2n )/(2n)! )
1757 .................... 0.04166666666667, 1777 .................... 0.04166666666667,
1758 .................... -0.00138888888889, 1778 .................... -0.00138888888889,
1759 .................... 0.00002480158730, 1779 .................... 0.00002480158730,
1760 .................... -0.00000027557319, 1780 .................... -0.00000027557319,
1761 .................... 0.00000000208767, 1781 .................... 0.00000000208767,
1762 .................... //-0.00000000001147, 1782 .................... //-0.00000000001147,
1763 .................... // 0.00000000000005 1783 .................... // 0.00000000000005
1764 .................... }; 1784 .................... };
1765 .................... 1785 ....................
1766 .................... if (x < 0) x = -x; // absolute value of input 1786 .................... if (x < 0) x = -x; // absolute value of input
1767 .................... 1787 ....................
1768 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant 1788 .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant
1769 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input 1789 .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input
1770 .................... quad = quad % 4; // quadrant (0 to 3) 1790 .................... quad = quad % 4; // quadrant (0 to 3)
1771 .................... 1791 ....................
1772 .................... if (quad == 0 || quad == 2) 1792 .................... if (quad == 0 || quad == 2)
1773 .................... t = frac * PI_DIV_BY_TWO; 1793 .................... t = frac * PI_DIV_BY_TWO;
1774 .................... else if (quad == 1) 1794 .................... else if (quad == 1)
1775 .................... t = (1-frac) * PI_DIV_BY_TWO; 1795 .................... t = (1-frac) * PI_DIV_BY_TWO;
1776 .................... else // should be 3 1796 .................... else // should be 3
1777 .................... t = (frac-1) * PI_DIV_BY_TWO; 1797 .................... t = (frac-1) * PI_DIV_BY_TWO;
1778 .................... 1798 ....................
1779 .................... y = 0.999999999781; 1799 .................... y = 0.999999999781;
1780 .................... t = t * t; 1800 .................... t = t * t;
1781 .................... for (i = 0; i <= 5; i++) 1801 .................... for (i = 0; i <= 5; i++)
1782 .................... { 1802 .................... {
1783 .................... t2 = t2 * t; 1803 .................... t2 = t2 * t;
1784 .................... y = y + p[i] * t2; 1804 .................... y = y + p[i] * t2;
1785 .................... } 1805 .................... }
1786 .................... 1806 ....................
1787 .................... if (quad == 2 || quad == 1) 1807 .................... if (quad == 2 || quad == 1)
1788 .................... y = -y; // correct sign 1808 .................... y = -y; // correct sign
1789 .................... 1809 ....................
1790 .................... return (y); 1810 .................... return (y);
1791 .................... } 1811 .................... }
1792 .................... 1812 ....................
1793 .................... #endif 1813 .................... #endif
1794 .................... 1814 ....................
1795 .................... //////////////////////////////////////////////////////////////////////////// 1815 .................... ////////////////////////////////////////////////////////////////////////////
1796 .................... // float sin(float x) 1816 .................... // float sin(float x)
1797 .................... //////////////////////////////////////////////////////////////////////////// 1817 .................... ////////////////////////////////////////////////////////////////////////////
1798 .................... // Description : returns the sine value of the angle x, which is in radian 1818 .................... // Description : returns the sine value of the angle x, which is in radian
1799 .................... // Date : 9/20/2001 1819 .................... // Date : 9/20/2001
1800 .................... // 1820 .................... //
1801 .................... float32 sin(float32 x) 1821 .................... float32 sin(float32 x)
1802 .................... { 1822 .................... {
1803 .................... return cos(x - PI_DIV_BY_TWO); 1823 .................... return cos(x - PI_DIV_BY_TWO);
1804 .................... } 1824 .................... }
1805 .................... 1825 ....................
1806 .................... //Overloaded functions for sin() for PCD 1826 .................... //Overloaded functions for sin() for PCD
1807 .................... // Overloaded function sin() for data type - Float48 1827 .................... // Overloaded function sin() for data type - Float48
1808 .................... #if defined(__PCD__) 1828 .................... #if defined(__PCD__)
1809 .................... float48 sin(float48 x) 1829 .................... float48 sin(float48 x)
1810 .................... { 1830 .................... {
1811 .................... return cos(x - PI_DIV_BY_TWO); 1831 .................... return cos(x - PI_DIV_BY_TWO);
1812 .................... } 1832 .................... }
1813 .................... 1833 ....................
1814 .................... // Overloaded function sin() for data type - Float48 1834 .................... // Overloaded function sin() for data type - Float48
1815 .................... float64 sin(float64 x) 1835 .................... float64 sin(float64 x)
1816 .................... { 1836 .................... {
1817 .................... return cos(x - PI_DIV_BY_TWO); 1837 .................... return cos(x - PI_DIV_BY_TWO);
1818 .................... } 1838 .................... }
1819 .................... #endif 1839 .................... #endif
1820 .................... 1840 ....................
1821 .................... //////////////////////////////////////////////////////////////////////////// 1841 .................... ////////////////////////////////////////////////////////////////////////////
1822 .................... // float tan(float x) 1842 .................... // float tan(float x)
1823 .................... //////////////////////////////////////////////////////////////////////////// 1843 .................... ////////////////////////////////////////////////////////////////////////////
1824 .................... // Description : returns the tangent value of the angle x, which is in radian 1844 .................... // Description : returns the tangent value of the angle x, which is in radian
1825 .................... // Date : 9/20/2001 1845 .................... // Date : 9/20/2001
1826 .................... // 1846 .................... //
1827 .................... float32 tan(float32 x) 1847 .................... float32 tan(float32 x)
1828 .................... { 1848 .................... {
1829 .................... float32 c, s; 1849 .................... float32 c, s;
1830 .................... 1850 ....................
1831 .................... c = cos(x); 1851 .................... c = cos(x);
1832 .................... if (c == 0.0) 1852 .................... if (c == 0.0)
1833 .................... return (1.0e+36); 1853 .................... return (1.0e+36);
1834 .................... 1854 ....................
1835 .................... s = sin(x); 1855 .................... s = sin(x);
1836 .................... return(s/c); 1856 .................... return(s/c);
1837 .................... } 1857 .................... }
1838 .................... //Overloaded functions for tan() for PCD 1858 .................... //Overloaded functions for tan() for PCD
1839 .................... // Overloaded function tan() for data type - Float48 1859 .................... // Overloaded function tan() for data type - Float48
1840 .................... #if defined(__PCD__) 1860 .................... #if defined(__PCD__)
1841 .................... float48 tan(float48 x) 1861 .................... float48 tan(float48 x)
1842 .................... { 1862 .................... {
1843 .................... float48 c, s; 1863 .................... float48 c, s;
1844 .................... 1864 ....................
1845 .................... c = cos(x); 1865 .................... c = cos(x);
1846 .................... if (c == 0.0) 1866 .................... if (c == 0.0)
1847 .................... return (1.0e+36); 1867 .................... return (1.0e+36);
1848 .................... 1868 ....................
1849 .................... s = sin(x); 1869 .................... s = sin(x);
1850 .................... return(s/c); 1870 .................... return(s/c);
1851 .................... } 1871 .................... }
1852 .................... 1872 ....................
1853 .................... // Overloaded function tan() for data type - Float48 1873 .................... // Overloaded function tan() for data type - Float48
1854 .................... float64 tan(float64 x) 1874 .................... float64 tan(float64 x)
1855 .................... { 1875 .................... {
1856 .................... float64 c, s; 1876 .................... float64 c, s;
1857 .................... 1877 ....................
1858 .................... c = cos(x); 1878 .................... c = cos(x);
1859 .................... if (c == 0.0) 1879 .................... if (c == 0.0)
1860 .................... return (1.0e+36); 1880 .................... return (1.0e+36);
1861 .................... 1881 ....................
1862 .................... s = sin(x); 1882 .................... s = sin(x);
1863 .................... return(s/c); 1883 .................... return(s/c);
1864 .................... } 1884 .................... }
1865 .................... #endif 1885 .................... #endif
1866 .................... 1886 ....................
1867 .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290}; 1887 .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290};
1868 .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290}; 1888 .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290};
1869 .................... 1889 ....................
1870 .................... float32 ASIN_COS(float32 x, unsigned int8 n) 1890 .................... float32 ASIN_COS(float32 x, unsigned int8 n)
1871 .................... { 1891 .................... {
1872 .................... float32 y, res, r, y2; 1892 .................... float32 y, res, r, y2;
1873 .................... int1 s; 1893 .................... int1 s;
1874 .................... #ifdef _ERRNO 1894 .................... #ifdef _ERRNO
1875 .................... if(x <-1 || x > 1) 1895 .................... if(x <-1 || x > 1)
1876 .................... { 1896 .................... {
1877 .................... errno=EDOM; 1897 .................... errno=EDOM;
1878 .................... } 1898 .................... }
1879 .................... #endif 1899 .................... #endif
1880 .................... s = 0; 1900 .................... s = 0;
1881 .................... y = x; 1901 .................... y = x;
1882 .................... 1902 ....................
1883 .................... if (x < 0) 1903 .................... if (x < 0)
1884 .................... { 1904 .................... {
1885 .................... s = 1; 1905 .................... s = 1;
1886 .................... y = -y; 1906 .................... y = -y;
1887 .................... } 1907 .................... }
1888 .................... 1908 ....................
1889 .................... if (y > 0.5) 1909 .................... if (y > 0.5)
1890 .................... { 1910 .................... {
1891 .................... y = sqrt((1.0 - y)/2.0); 1911 .................... y = sqrt((1.0 - y)/2.0);
1892 .................... n += 2; 1912 .................... n += 2;
1893 .................... } 1913 .................... }
1894 .................... 1914 ....................
1895 .................... y2=y*y; 1915 .................... y2=y*y;
1896 .................... 1916 ....................
1897 .................... res = pas[0]*y2 + pas[1]; 1917 .................... res = pas[0]*y2 + pas[1];
1898 .................... res = res*y2 + pas[2]; 1918 .................... res = res*y2 + pas[2];
1899 .................... 1919 ....................
1900 .................... r = qas[0]*y2 + qas[1]; 1920 .................... r = qas[0]*y2 + qas[1];
1901 .................... r = r*y2 + qas[2]; 1921 .................... r = r*y2 + qas[2];
1902 .................... 1922 ....................
1903 .................... res = y*res/r; 1923 .................... res = y*res/r;
1904 .................... 1924 ....................
1905 .................... if (n & 2) // |x| > 0.5 1925 .................... if (n & 2) // |x| > 0.5
1906 .................... res = PI_DIV_BY_TWO - 2.0*res; 1926 .................... res = PI_DIV_BY_TWO - 2.0*res;
1907 .................... if (s) 1927 .................... if (s)
1908 .................... res = -res; 1928 .................... res = -res;
1909 .................... if (n & 1) // take arccos 1929 .................... if (n & 1) // take arccos
1910 .................... res = PI_DIV_BY_TWO - res; 1930 .................... res = PI_DIV_BY_TWO - res;
1911 .................... 1931 ....................
1912 .................... return(res); 1932 .................... return(res);
1913 .................... } 1933 .................... }
1914 .................... 1934 ....................
1915 .................... //Overloaded functions for ASIN_COS() for PCD 1935 .................... //Overloaded functions for ASIN_COS() for PCD
1916 .................... // Overloaded function ASIN_COS() for data type - Float48 1936 .................... // Overloaded function ASIN_COS() for data type - Float48
1917 .................... #if defined(__PCD__) 1937 .................... #if defined(__PCD__)
1918 .................... float48 ASIN_COS(float48 x, unsigned int8 n) 1938 .................... float48 ASIN_COS(float48 x, unsigned int8 n)
1919 .................... { 1939 .................... {
1920 .................... float48 y, res, r, y2; 1940 .................... float48 y, res, r, y2;
1921 .................... int1 s; 1941 .................... int1 s;
1922 .................... #ifdef _ERRNO 1942 .................... #ifdef _ERRNO
1923 .................... if(x <-1 || x > 1) 1943 .................... if(x <-1 || x > 1)
1924 .................... { 1944 .................... {
1925 .................... errno=EDOM; 1945 .................... errno=EDOM;
1926 .................... } 1946 .................... }
1927 .................... #endif 1947 .................... #endif
1928 .................... s = 0; 1948 .................... s = 0;
1929 .................... y = x; 1949 .................... y = x;
1930 .................... 1950 ....................
1931 .................... if (x < 0) 1951 .................... if (x < 0)
1932 .................... { 1952 .................... {
1933 .................... s = 1; 1953 .................... s = 1;
1934 .................... y = -y; 1954 .................... y = -y;
1935 .................... } 1955 .................... }
1936 .................... 1956 ....................
1937 .................... if (y > 0.5) 1957 .................... if (y > 0.5)
1938 .................... { 1958 .................... {
1939 .................... y = sqrt((1.0 - y)/2.0); 1959 .................... y = sqrt((1.0 - y)/2.0);
1940 .................... n += 2; 1960 .................... n += 2;
1941 .................... } 1961 .................... }
1942 .................... 1962 ....................
1943 .................... y2=y*y; 1963 .................... y2=y*y;
1944 .................... 1964 ....................
1945 .................... res = pas[0]*y2 + pas[1]; 1965 .................... res = pas[0]*y2 + pas[1];
1946 .................... res = res*y2 + pas[2]; 1966 .................... res = res*y2 + pas[2];
1947 .................... 1967 ....................
1948 .................... r = qas[0]*y2 + qas[1]; 1968 .................... r = qas[0]*y2 + qas[1];
1949 .................... r = r*y2 + qas[2]; 1969 .................... r = r*y2 + qas[2];
1950 .................... 1970 ....................
1951 .................... res = y*res/r; 1971 .................... res = y*res/r;
1952 .................... 1972 ....................
1953 .................... if (n & 2) // |x| > 0.5 1973 .................... if (n & 2) // |x| > 0.5
1954 .................... res = PI_DIV_BY_TWO - 2.0*res; 1974 .................... res = PI_DIV_BY_TWO - 2.0*res;
1955 .................... if (s) 1975 .................... if (s)
1956 .................... res = -res; 1976 .................... res = -res;
1957 .................... if (n & 1) // take arccos 1977 .................... if (n & 1) // take arccos
1958 .................... res = PI_DIV_BY_TWO - res; 1978 .................... res = PI_DIV_BY_TWO - res;
1959 .................... 1979 ....................
1960 .................... return(res); 1980 .................... return(res);
1961 .................... } 1981 .................... }
1962 .................... 1982 ....................
1963 .................... // Overloaded function ASIN_COS() for data type - Float64 1983 .................... // Overloaded function ASIN_COS() for data type - Float64
1964 .................... float64 ASIN_COS(float64 x, unsigned int8 n) 1984 .................... float64 ASIN_COS(float64 x, unsigned int8 n)
1965 .................... { 1985 .................... {
1966 .................... float64 y, res, r, y2; 1986 .................... float64 y, res, r, y2;
1967 .................... int1 s; 1987 .................... int1 s;
1968 .................... #ifdef _ERRNO 1988 .................... #ifdef _ERRNO
1969 .................... if(x <-1 || x > 1) 1989 .................... if(x <-1 || x > 1)
1970 .................... { 1990 .................... {
1971 .................... errno=EDOM; 1991 .................... errno=EDOM;
1972 .................... } 1992 .................... }
1973 .................... #endif 1993 .................... #endif
1974 .................... s = 0; 1994 .................... s = 0;
1975 .................... y = x; 1995 .................... y = x;
1976 .................... 1996 ....................
1977 .................... if (x < 0) 1997 .................... if (x < 0)
1978 .................... { 1998 .................... {
1979 .................... s = 1; 1999 .................... s = 1;
1980 .................... y = -y; 2000 .................... y = -y;
1981 .................... } 2001 .................... }
1982 .................... 2002 ....................
1983 .................... if (y > 0.5) 2003 .................... if (y > 0.5)
1984 .................... { 2004 .................... {
1985 .................... y = sqrt((1.0 - y)/2.0); 2005 .................... y = sqrt((1.0 - y)/2.0);
1986 .................... n += 2; 2006 .................... n += 2;
1987 .................... } 2007 .................... }
1988 .................... 2008 ....................
1989 .................... y2=y*y; 2009 .................... y2=y*y;
1990 .................... 2010 ....................
1991 .................... res = pas[0]*y2 + pas[1]; 2011 .................... res = pas[0]*y2 + pas[1];
1992 .................... res = res*y2 + pas[2]; 2012 .................... res = res*y2 + pas[2];
1993 .................... 2013 ....................
1994 .................... r = qas[0]*y2 + qas[1]; 2014 .................... r = qas[0]*y2 + qas[1];
1995 .................... r = r*y2 + qas[2]; 2015 .................... r = r*y2 + qas[2];
1996 .................... 2016 ....................
1997 .................... res = y*res/r; 2017 .................... res = y*res/r;
1998 .................... 2018 ....................
1999 .................... if (n & 2) // |x| > 0.5 2019 .................... if (n & 2) // |x| > 0.5
2000 .................... res = PI_DIV_BY_TWO - 2.0*res; 2020 .................... res = PI_DIV_BY_TWO - 2.0*res;
2001 .................... if (s) 2021 .................... if (s)
2002 .................... res = -res; 2022 .................... res = -res;
2003 .................... if (n & 1) // take arccos 2023 .................... if (n & 1) // take arccos
2004 .................... res = PI_DIV_BY_TWO - res; 2024 .................... res = PI_DIV_BY_TWO - res;
2005 .................... 2025 ....................
2006 .................... return(res); 2026 .................... return(res);
2007 .................... } 2027 .................... }
2008 .................... #endif 2028 .................... #endif
2009 .................... 2029 ....................
2010 .................... //////////////////////////////////////////////////////////////////////////// 2030 .................... ////////////////////////////////////////////////////////////////////////////
2011 .................... // float asin(float x) 2031 .................... // float asin(float x)
2012 .................... //////////////////////////////////////////////////////////////////////////// 2032 .................... ////////////////////////////////////////////////////////////////////////////
2013 .................... // Description : returns the arcsine value of the value x. 2033 .................... // Description : returns the arcsine value of the value x.
2014 .................... // Date : N/A 2034 .................... // Date : N/A
2015 .................... // 2035 .................... //
2016 .................... float32 asin(float32 x) 2036 .................... float32 asin(float32 x)
2017 .................... { 2037 .................... {
2018 .................... float32 r; 2038 .................... float32 r;
2019 .................... 2039 ....................
2020 .................... r = ASIN_COS(x, 0); 2040 .................... r = ASIN_COS(x, 0);
2021 .................... return(r); 2041 .................... return(r);
2022 .................... } 2042 .................... }
2023 .................... //Overloaded functions for asin() for PCD 2043 .................... //Overloaded functions for asin() for PCD
2024 .................... // Overloaded function asin() for data type - Float48 2044 .................... // Overloaded function asin() for data type - Float48
2025 .................... #if defined(__PCD__) 2045 .................... #if defined(__PCD__)
2026 .................... float48 asin(float48 x) 2046 .................... float48 asin(float48 x)
2027 .................... { 2047 .................... {
2028 .................... float48 r; 2048 .................... float48 r;
2029 .................... 2049 ....................
2030 .................... r = ASIN_COS(x, 0); 2050 .................... r = ASIN_COS(x, 0);
2031 .................... return(r); 2051 .................... return(r);
2032 .................... } 2052 .................... }
2033 .................... 2053 ....................
2034 .................... // Overloaded function asin() for data type - Float64 2054 .................... // Overloaded function asin() for data type - Float64
2035 .................... float64 asin(float64 x) 2055 .................... float64 asin(float64 x)
2036 .................... { 2056 .................... {
2037 .................... float64 r; 2057 .................... float64 r;
2038 .................... 2058 ....................
2039 .................... r = ASIN_COS(x, 0); 2059 .................... r = ASIN_COS(x, 0);
2040 .................... return(r); 2060 .................... return(r);
2041 .................... } 2061 .................... }
2042 .................... #endif 2062 .................... #endif
2043 .................... 2063 ....................
2044 .................... //////////////////////////////////////////////////////////////////////////// 2064 .................... ////////////////////////////////////////////////////////////////////////////
2045 .................... // float acos(float x) 2065 .................... // float acos(float x)
2046 .................... //////////////////////////////////////////////////////////////////////////// 2066 .................... ////////////////////////////////////////////////////////////////////////////
2047 .................... // Description : returns the arccosine value of the value x. 2067 .................... // Description : returns the arccosine value of the value x.
2048 .................... // Date : N/A 2068 .................... // Date : N/A
2049 .................... // 2069 .................... //
2050 .................... float32 acos(float32 x) 2070 .................... float32 acos(float32 x)
2051 .................... { 2071 .................... {
2052 .................... float32 r; 2072 .................... float32 r;
2053 .................... 2073 ....................
2054 .................... r = ASIN_COS(x, 1); 2074 .................... r = ASIN_COS(x, 1);
2055 .................... return(r); 2075 .................... return(r);
2056 .................... } 2076 .................... }
2057 .................... //Overloaded functions for acos() for PCD 2077 .................... //Overloaded functions for acos() for PCD
2058 .................... // Overloaded function acos() for data type - Float48 2078 .................... // Overloaded function acos() for data type - Float48
2059 .................... #if defined(__PCD__) 2079 .................... #if defined(__PCD__)
2060 .................... float48 acos(float48 x) 2080 .................... float48 acos(float48 x)
2061 .................... { 2081 .................... {
2062 .................... float48 r; 2082 .................... float48 r;
2063 .................... 2083 ....................
2064 .................... r = ASIN_COS(x, 1); 2084 .................... r = ASIN_COS(x, 1);
2065 .................... return(r); 2085 .................... return(r);
2066 .................... } 2086 .................... }
2067 .................... 2087 ....................
2068 .................... // Overloaded function acos() for data type - Float64 2088 .................... // Overloaded function acos() for data type - Float64
2069 .................... float64 acos(float64 x) 2089 .................... float64 acos(float64 x)
2070 .................... { 2090 .................... {
2071 .................... float64 r; 2091 .................... float64 r;
2072 .................... 2092 ....................
2073 .................... r = ASIN_COS(x, 1); 2093 .................... r = ASIN_COS(x, 1);
2074 .................... return(r); 2094 .................... return(r);
2075 .................... } 2095 .................... }
2076 .................... #endif 2096 .................... #endif
2077 .................... 2097 ....................
2078 .................... float32 const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; 2098 .................... 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}; 2099 .................... float32 const qat[4] = {1.0000000, 11.368190, 28.982246, 19.818457};
2080 .................... 2100 ....................
2081 .................... //////////////////////////////////////////////////////////////////////////// 2101 .................... ////////////////////////////////////////////////////////////////////////////
2082 .................... // float atan(float x) 2102 .................... // float atan(float x)
2083 .................... //////////////////////////////////////////////////////////////////////////// 2103 .................... ////////////////////////////////////////////////////////////////////////////
2084 .................... // Description : returns the arctangent value of the value x. 2104 .................... // Description : returns the arctangent value of the value x.
2085 .................... // Date : N/A 2105 .................... // Date : N/A
2086 .................... // 2106 .................... //
2087 .................... float32 atan(float32 x) 2107 .................... float32 atan(float32 x)
2088 .................... { 2108 .................... {
2089 .................... float32 y, res, r; 2109 .................... float32 y, res, r;
2090 .................... int1 s, flag; 2110 .................... int1 s, flag;
2091 .................... 2111 ....................
2092 .................... s = 0; 2112 .................... s = 0;
2093 .................... flag = 0; 2113 .................... flag = 0;
2094 .................... y = x; 2114 .................... y = x;
2095 .................... 2115 ....................
2096 .................... if (x < 0) 2116 .................... if (x < 0)
2097 .................... { 2117 .................... {
2098 .................... s = 1; 2118 .................... s = 1;
2099 .................... y = -y; 2119 .................... y = -y;
2100 .................... } 2120 .................... }
2101 .................... 2121 ....................
2102 .................... if (y > 1.0) 2122 .................... if (y > 1.0)
2103 .................... { 2123 .................... {
2104 .................... y = 1.0/y; 2124 .................... y = 1.0/y;
2105 .................... flag = 1; 2125 .................... flag = 1;
2106 .................... } 2126 .................... }
2107 .................... 2127 ....................
2108 .................... res = pat[0]*y*y + pat[1]; 2128 .................... res = pat[0]*y*y + pat[1];
2109 .................... res = res*y*y + pat[2]; 2129 .................... res = res*y*y + pat[2];
2110 .................... res = res*y*y + pat[3]; 2130 .................... res = res*y*y + pat[3];
2111 .................... 2131 ....................
2112 .................... r = qat[0]*y*y + qat[1]; 2132 .................... r = qat[0]*y*y + qat[1];
2113 .................... r = r*y*y + qat[2]; 2133 .................... r = r*y*y + qat[2];
2114 .................... r = r*y*y + qat[3]; 2134 .................... r = r*y*y + qat[3];
2115 .................... 2135 ....................
2116 .................... res = y*res/r; 2136 .................... res = y*res/r;
2117 .................... 2137 ....................
2118 .................... 2138 ....................
2119 .................... if (flag) // for |x| > 1 2139 .................... if (flag) // for |x| > 1
2120 .................... res = PI_DIV_BY_TWO - res; 2140 .................... res = PI_DIV_BY_TWO - res;
2121 .................... if (s) 2141 .................... if (s)
2122 .................... res = -res; 2142 .................... res = -res;
2123 .................... 2143 ....................
2124 .................... return(res); 2144 .................... return(res);
2125 .................... } 2145 .................... }
2126 .................... //Overloaded functions for atan() for PCD 2146 .................... //Overloaded functions for atan() for PCD
2127 .................... // Overloaded function atan() for data type - Float48 2147 .................... // Overloaded function atan() for data type - Float48
2128 .................... #if defined(__PCD__) 2148 .................... #if defined(__PCD__)
2129 .................... float48 atan(float48 x) 2149 .................... float48 atan(float48 x)
2130 .................... { 2150 .................... {
2131 .................... float48 y, res, r; 2151 .................... float48 y, res, r;
2132 .................... int1 s, flag; 2152 .................... int1 s, flag;
2133 .................... 2153 ....................
2134 .................... s = 0; 2154 .................... s = 0;
2135 .................... flag = 0; 2155 .................... flag = 0;
2136 .................... y = x; 2156 .................... y = x;
2137 .................... 2157 ....................
2138 .................... if (x < 0) 2158 .................... if (x < 0)
2139 .................... { 2159 .................... {
2140 .................... s = 1; 2160 .................... s = 1;
2141 .................... y = -y; 2161 .................... y = -y;
2142 .................... } 2162 .................... }
2143 .................... 2163 ....................
2144 .................... if (y > 1.0) 2164 .................... if (y > 1.0)
2145 .................... { 2165 .................... {
2146 .................... y = 1.0/y; 2166 .................... y = 1.0/y;
2147 .................... flag = 1; 2167 .................... flag = 1;
2148 .................... } 2168 .................... }
2149 .................... 2169 ....................
2150 .................... res = pat[0]*y*y + pat[1]; 2170 .................... res = pat[0]*y*y + pat[1];
2151 .................... res = res*y*y + pat[2]; 2171 .................... res = res*y*y + pat[2];
2152 .................... res = res*y*y + pat[3]; 2172 .................... res = res*y*y + pat[3];
2153 .................... 2173 ....................
2154 .................... r = qat[0]*y*y + qat[1]; 2174 .................... r = qat[0]*y*y + qat[1];
2155 .................... r = r*y*y + qat[2]; 2175 .................... r = r*y*y + qat[2];
2156 .................... r = r*y*y + qat[3]; 2176 .................... r = r*y*y + qat[3];
2157 .................... 2177 ....................
2158 .................... res = y*res/r; 2178 .................... res = y*res/r;
2159 .................... 2179 ....................
2160 .................... 2180 ....................
2161 .................... if (flag) // for |x| > 1 2181 .................... if (flag) // for |x| > 1
2162 .................... res = PI_DIV_BY_TWO - res; 2182 .................... res = PI_DIV_BY_TWO - res;
2163 .................... if (s) 2183 .................... if (s)
2164 .................... res = -res; 2184 .................... res = -res;
2165 .................... 2185 ....................
2166 .................... return(res); 2186 .................... return(res);
2167 .................... } 2187 .................... }
2168 .................... 2188 ....................
2169 .................... // Overloaded function atan() for data type - Float64 2189 .................... // Overloaded function atan() for data type - Float64
2170 .................... float64 atan(float64 x) 2190 .................... float64 atan(float64 x)
2171 .................... { 2191 .................... {
2172 .................... float64 y, res, r; 2192 .................... float64 y, res, r;
2173 .................... int1 s, flag; 2193 .................... int1 s, flag;
2174 .................... 2194 ....................
2175 .................... s = 0; 2195 .................... s = 0;
2176 .................... flag = 0; 2196 .................... flag = 0;
2177 .................... y = x; 2197 .................... y = x;
2178 .................... 2198 ....................
2179 .................... if (x < 0) 2199 .................... if (x < 0)
2180 .................... { 2200 .................... {
2181 .................... s = 1; 2201 .................... s = 1;
2182 .................... y = -y; 2202 .................... y = -y;
2183 .................... } 2203 .................... }
2184 .................... 2204 ....................
2185 .................... if (y > 1.0) 2205 .................... if (y > 1.0)
2186 .................... { 2206 .................... {
2187 .................... y = 1.0/y; 2207 .................... y = 1.0/y;
2188 .................... flag = 1; 2208 .................... flag = 1;
2189 .................... } 2209 .................... }
2190 .................... 2210 ....................
2191 .................... res = pat[0]*y*y + pat[1]; 2211 .................... res = pat[0]*y*y + pat[1];
2192 .................... res = res*y*y + pat[2]; 2212 .................... res = res*y*y + pat[2];
2193 .................... res = res*y*y + pat[3]; 2213 .................... res = res*y*y + pat[3];
2194 .................... 2214 ....................
2195 .................... r = qat[0]*y*y + qat[1]; 2215 .................... r = qat[0]*y*y + qat[1];
2196 .................... r = r*y*y + qat[2]; 2216 .................... r = r*y*y + qat[2];
2197 .................... r = r*y*y + qat[3]; 2217 .................... r = r*y*y + qat[3];
2198 .................... 2218 ....................
2199 .................... res = y*res/r; 2219 .................... res = y*res/r;
2200 .................... 2220 ....................
2201 .................... 2221 ....................
2202 .................... if (flag) // for |x| > 1 2222 .................... if (flag) // for |x| > 1
2203 .................... res = PI_DIV_BY_TWO - res; 2223 .................... res = PI_DIV_BY_TWO - res;
2204 .................... if (s) 2224 .................... if (s)
2205 .................... res = -res; 2225 .................... res = -res;
2206 .................... 2226 ....................
2207 .................... return(res); 2227 .................... return(res);
2208 .................... } 2228 .................... }
2209 .................... #endif 2229 .................... #endif
2210 .................... 2230 ....................
2211 .................... ///////////////////////////////////////////////////////////////////////////// 2231 .................... /////////////////////////////////////////////////////////////////////////////
2212 .................... // float atan2(float y, float x) 2232 .................... // float atan2(float y, float x)
2213 .................... ///////////////////////////////////////////////////////////////////////////// 2233 .................... /////////////////////////////////////////////////////////////////////////////
2214 .................... // Description :computes the principal value of arc tangent of y/x, using the 2234 .................... // 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 2235 .................... // signs of both the arguments to determine the quadrant of the return value
2216 .................... // Returns : returns the arc tangent of y/x. 2236 .................... // Returns : returns the arc tangent of y/x.
2217 .................... // Date : N/A 2237 .................... // Date : N/A
2218 .................... // 2238 .................... //
2219 .................... 2239 ....................
2220 .................... float32 atan2(float32 y,float32 x) 2240 .................... float32 atan2(float32 y,float32 x)
2221 .................... { 2241 .................... {
2222 .................... float32 z; 2242 .................... float32 z;
2223 .................... int1 sign; 2243 .................... int1 sign;
2224 .................... unsigned int8 quad; 2244 .................... unsigned int8 quad;
2225 .................... sign=0; 2245 .................... sign=0;
2226 .................... quad=0; //quadrant 2246 .................... quad=0; //quadrant
2227 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 2247 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2228 .................... if(y<0.0) 2248 .................... if(y<0.0)
2229 .................... { 2249 .................... {
2230 .................... sign=1; 2250 .................... sign=1;
2231 .................... y=-y; 2251 .................... y=-y;
2232 .................... } 2252 .................... }
2233 .................... if(x<0.0) 2253 .................... if(x<0.0)
2234 .................... { 2254 .................... {
2235 .................... x=-x; 2255 .................... x=-x;
2236 .................... } 2256 .................... }
2237 .................... if (x==0.0) 2257 .................... if (x==0.0)
2238 .................... { 2258 .................... {
2239 .................... if(y==0.0) 2259 .................... if(y==0.0)
2240 .................... { 2260 .................... {
2241 .................... #ifdef _ERRNO 2261 .................... #ifdef _ERRNO
2242 .................... { 2262 .................... {
2243 .................... errno=EDOM; 2263 .................... errno=EDOM;
2244 .................... } 2264 .................... }
2245 .................... #endif 2265 .................... #endif
2246 .................... } 2266 .................... }
2247 .................... else 2267 .................... else
2248 .................... { 2268 .................... {
2249 .................... if(sign) 2269 .................... if(sign)
2250 .................... { 2270 .................... {
2251 .................... return (-(PI_DIV_BY_TWO)); 2271 .................... return (-(PI_DIV_BY_TWO));
2252 .................... } 2272 .................... }
2253 .................... else 2273 .................... else
2254 .................... { 2274 .................... {
2255 .................... return (PI_DIV_BY_TWO); 2275 .................... return (PI_DIV_BY_TWO);
2256 .................... } 2276 .................... }
2257 .................... } 2277 .................... }
2258 .................... } 2278 .................... }
2259 .................... else 2279 .................... else
2260 .................... { 2280 .................... {
2261 .................... z=y/x; 2281 .................... z=y/x;
2262 .................... switch(quad) 2282 .................... switch(quad)
2263 .................... { 2283 .................... {
2264 .................... case 1: 2284 .................... case 1:
2265 .................... { 2285 .................... {
2266 .................... return atan(z); 2286 .................... return atan(z);
2267 .................... break; 2287 .................... break;
2268 .................... } 2288 .................... }
2269 .................... case 2: 2289 .................... case 2:
2270 .................... { 2290 .................... {
2271 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 2291 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2272 .................... return (PI-atan(z)); 2292 .................... return (PI-atan(z));
2273 .................... break; 2293 .................... break;
2274 .................... } 2294 .................... }
2275 .................... case 3: 2295 .................... case 3:
2276 .................... { 2296 .................... {
2277 .................... return (atan(z)-PI); 2297 .................... return (atan(z)-PI);
2278 .................... break; 2298 .................... break;
2279 .................... } 2299 .................... }
2280 .................... case 4: 2300 .................... case 4:
2281 .................... { 2301 .................... {
2282 .................... return (-atan(z)); 2302 .................... return (-atan(z));
2283 .................... break; 2303 .................... break;
2284 .................... } 2304 .................... }
2285 .................... } 2305 .................... }
2286 .................... } 2306 .................... }
2287 .................... } 2307 .................... }
2288 .................... 2308 ....................
2289 .................... //Overloaded functions for atan2() for PCD 2309 .................... //Overloaded functions for atan2() for PCD
2290 .................... // Overloaded function atan2() for data type - Float48 2310 .................... // Overloaded function atan2() for data type - Float48
2291 .................... #if defined(__PCD__) 2311 .................... #if defined(__PCD__)
2292 .................... float48 atan2(float48 y,float48 x) 2312 .................... float48 atan2(float48 y,float48 x)
2293 .................... { 2313 .................... {
2294 .................... float48 z; 2314 .................... float48 z;
2295 .................... int1 sign; 2315 .................... int1 sign;
2296 .................... unsigned int8 quad; 2316 .................... unsigned int8 quad;
2297 .................... sign=0; 2317 .................... sign=0;
2298 .................... quad=0; //quadrant 2318 .................... quad=0; //quadrant
2299 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 2319 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2300 .................... if(y<0.0) 2320 .................... if(y<0.0)
2301 .................... { 2321 .................... {
2302 .................... sign=1; 2322 .................... sign=1;
2303 .................... y=-y; 2323 .................... y=-y;
2304 .................... } 2324 .................... }
2305 .................... if(x<0.0) 2325 .................... if(x<0.0)
2306 .................... { 2326 .................... {
2307 .................... x=-x; 2327 .................... x=-x;
2308 .................... } 2328 .................... }
2309 .................... if (x==0.0) 2329 .................... if (x==0.0)
2310 .................... { 2330 .................... {
2311 .................... if(y==0.0) 2331 .................... if(y==0.0)
2312 .................... { 2332 .................... {
2313 .................... #ifdef _ERRNO 2333 .................... #ifdef _ERRNO
2314 .................... { 2334 .................... {
2315 .................... errno=EDOM; 2335 .................... errno=EDOM;
2316 .................... } 2336 .................... }
2317 .................... #endif 2337 .................... #endif
2318 .................... } 2338 .................... }
2319 .................... else 2339 .................... else
2320 .................... { 2340 .................... {
2321 .................... if(sign) 2341 .................... if(sign)
2322 .................... { 2342 .................... {
2323 .................... return (-(PI_DIV_BY_TWO)); 2343 .................... return (-(PI_DIV_BY_TWO));
2324 .................... } 2344 .................... }
2325 .................... else 2345 .................... else
2326 .................... { 2346 .................... {
2327 .................... return (PI_DIV_BY_TWO); 2347 .................... return (PI_DIV_BY_TWO);
2328 .................... } 2348 .................... }
2329 .................... } 2349 .................... }
2330 .................... } 2350 .................... }
2331 .................... else 2351 .................... else
2332 .................... { 2352 .................... {
2333 .................... z=y/x; 2353 .................... z=y/x;
2334 .................... switch(quad) 2354 .................... switch(quad)
2335 .................... { 2355 .................... {
2336 .................... case 1: 2356 .................... case 1:
2337 .................... { 2357 .................... {
2338 .................... return atan(z); 2358 .................... return atan(z);
2339 .................... break; 2359 .................... break;
2340 .................... } 2360 .................... }
2341 .................... case 2: 2361 .................... case 2:
2342 .................... { 2362 .................... {
2343 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 2363 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2344 .................... return (PI-atan(z)); 2364 .................... return (PI-atan(z));
2345 .................... break; 2365 .................... break;
2346 .................... } 2366 .................... }
2347 .................... case 3: 2367 .................... case 3:
2348 .................... { 2368 .................... {
2349 .................... return (atan(z)-PI); 2369 .................... return (atan(z)-PI);
2350 .................... break; 2370 .................... break;
2351 .................... } 2371 .................... }
2352 .................... case 4: 2372 .................... case 4:
2353 .................... { 2373 .................... {
2354 .................... return (-atan(z)); 2374 .................... return (-atan(z));
2355 .................... break; 2375 .................... break;
2356 .................... } 2376 .................... }
2357 .................... } 2377 .................... }
2358 .................... } 2378 .................... }
2359 .................... } 2379 .................... }
2360 .................... 2380 ....................
2361 .................... // Overloaded function atan2() for data type - Float64 2381 .................... // Overloaded function atan2() for data type - Float64
2362 .................... float64 atan2(float64 y,float64 x) 2382 .................... float64 atan2(float64 y,float64 x)
2363 .................... { 2383 .................... {
2364 .................... float64 z; 2384 .................... float64 z;
2365 .................... int1 sign; 2385 .................... int1 sign;
2366 .................... unsigned int8 quad; 2386 .................... unsigned int8 quad;
2367 .................... sign=0; 2387 .................... sign=0;
2368 .................... quad=0; //quadrant 2388 .................... quad=0; //quadrant
2369 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 2389 .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1));
2370 .................... if(y<0.0) 2390 .................... if(y<0.0)
2371 .................... { 2391 .................... {
2372 .................... sign=1; 2392 .................... sign=1;
2373 .................... y=-y; 2393 .................... y=-y;
2374 .................... } 2394 .................... }
2375 .................... if(x<0.0) 2395 .................... if(x<0.0)
2376 .................... { 2396 .................... {
2377 .................... x=-x; 2397 .................... x=-x;
2378 .................... } 2398 .................... }
2379 .................... if (x==0.0) 2399 .................... if (x==0.0)
2380 .................... { 2400 .................... {
2381 .................... if(y==0.0) 2401 .................... if(y==0.0)
2382 .................... { 2402 .................... {
2383 .................... #ifdef _ERRNO 2403 .................... #ifdef _ERRNO
2384 .................... { 2404 .................... {
2385 .................... errno=EDOM; 2405 .................... errno=EDOM;
2386 .................... } 2406 .................... }
2387 .................... #endif 2407 .................... #endif
2388 .................... } 2408 .................... }
2389 .................... else 2409 .................... else
2390 .................... { 2410 .................... {
2391 .................... if(sign) 2411 .................... if(sign)
2392 .................... { 2412 .................... {
2393 .................... return (-(PI_DIV_BY_TWO)); 2413 .................... return (-(PI_DIV_BY_TWO));
2394 .................... } 2414 .................... }
2395 .................... else 2415 .................... else
2396 .................... { 2416 .................... {
2397 .................... return (PI_DIV_BY_TWO); 2417 .................... return (PI_DIV_BY_TWO);
2398 .................... } 2418 .................... }
2399 .................... } 2419 .................... }
2400 .................... } 2420 .................... }
2401 .................... else 2421 .................... else
2402 .................... { 2422 .................... {
2403 .................... z=y/x; 2423 .................... z=y/x;
2404 .................... switch(quad) 2424 .................... switch(quad)
2405 .................... { 2425 .................... {
2406 .................... case 1: 2426 .................... case 1:
2407 .................... { 2427 .................... {
2408 .................... return atan(z); 2428 .................... return atan(z);
2409 .................... break; 2429 .................... break;
2410 .................... } 2430 .................... }
2411 .................... case 2: 2431 .................... case 2:
2412 .................... { 2432 .................... {
2413 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 2433 .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122
2414 .................... return (PI-atan(z)); 2434 .................... return (PI-atan(z));
2415 .................... break; 2435 .................... break;
2416 .................... } 2436 .................... }
2417 .................... case 3: 2437 .................... case 3:
2418 .................... { 2438 .................... {
2419 .................... return (atan(z)-PI); 2439 .................... return (atan(z)-PI);
2420 .................... break; 2440 .................... break;
2421 .................... } 2441 .................... }
2422 .................... case 4: 2442 .................... case 4:
2423 .................... { 2443 .................... {
2424 .................... return (-atan(z)); 2444 .................... return (-atan(z));
2425 .................... break; 2445 .................... break;
2426 .................... } 2446 .................... }
2427 .................... } 2447 .................... }
2428 .................... } 2448 .................... }
2429 .................... } 2449 .................... }
2430 .................... #endif 2450 .................... #endif
2431 .................... 2451 ....................
2432 .................... //////////////////// Hyperbolic functions //////////////////// 2452 .................... //////////////////// Hyperbolic functions ////////////////////
2433 .................... 2453 ....................
2434 .................... //////////////////////////////////////////////////////////////////////////// 2454 .................... ////////////////////////////////////////////////////////////////////////////
2435 .................... // float cosh(float x) 2455 .................... // float cosh(float x)
2436 .................... //////////////////////////////////////////////////////////////////////////// 2456 .................... ////////////////////////////////////////////////////////////////////////////
2437 .................... // Description : Computes the hyperbolic cosine value of x 2457 .................... // Description : Computes the hyperbolic cosine value of x
2438 .................... // Returns : returns the hyperbolic cosine value of x 2458 .................... // Returns : returns the hyperbolic cosine value of x
2439 .................... // Date : N/A 2459 .................... // Date : N/A
2440 .................... // 2460 .................... //
2441 .................... 2461 ....................
2442 .................... float32 cosh(float32 x) 2462 .................... float32 cosh(float32 x)
2443 .................... { 2463 .................... {
2444 .................... return ((exp(x)+exp(-x))/2); 2464 .................... return ((exp(x)+exp(-x))/2);
2445 .................... } 2465 .................... }
2446 .................... //Overloaded functions for cosh() for PCD 2466 .................... //Overloaded functions for cosh() for PCD
2447 .................... // Overloaded function cosh() for data type - Float48 2467 .................... // Overloaded function cosh() for data type - Float48
2448 .................... #if defined(__PCD__) 2468 .................... #if defined(__PCD__)
2449 .................... float48 cosh(float48 x) 2469 .................... float48 cosh(float48 x)
2450 .................... { 2470 .................... {
2451 .................... return ((exp(x)+exp(-x))/2); 2471 .................... return ((exp(x)+exp(-x))/2);
2452 .................... } 2472 .................... }
2453 .................... 2473 ....................
2454 .................... // Overloaded function cosh() for data type - Float64 2474 .................... // Overloaded function cosh() for data type - Float64
2455 .................... float64 cosh(float64 x) 2475 .................... float64 cosh(float64 x)
2456 .................... { 2476 .................... {
2457 .................... return ((exp(x)+exp(-x))/2); 2477 .................... return ((exp(x)+exp(-x))/2);
2458 .................... } 2478 .................... }
2459 .................... #endif 2479 .................... #endif
2460 .................... 2480 ....................
2461 .................... //////////////////////////////////////////////////////////////////////////// 2481 .................... ////////////////////////////////////////////////////////////////////////////
2462 .................... // float sinh(float x) 2482 .................... // float sinh(float x)
2463 .................... //////////////////////////////////////////////////////////////////////////// 2483 .................... ////////////////////////////////////////////////////////////////////////////
2464 .................... // Description : Computes the hyperbolic sine value of x 2484 .................... // Description : Computes the hyperbolic sine value of x
2465 .................... // Returns : returns the hyperbolic sine value of x 2485 .................... // Returns : returns the hyperbolic sine value of x
2466 .................... // Date : N/A 2486 .................... // Date : N/A
2467 .................... // 2487 .................... //
2468 .................... 2488 ....................
2469 .................... float32 sinh(float32 x) 2489 .................... float32 sinh(float32 x)
2470 .................... { 2490 .................... {
2471 .................... 2491 ....................
2472 .................... return ((exp(x) - exp(-x))/2); 2492 .................... return ((exp(x) - exp(-x))/2);
2473 .................... } 2493 .................... }
2474 .................... //Overloaded functions for sinh() for PCD 2494 .................... //Overloaded functions for sinh() for PCD
2475 .................... // Overloaded function sinh() for data type - Float48 2495 .................... // Overloaded function sinh() for data type - Float48
2476 .................... #if defined(__PCD__) 2496 .................... #if defined(__PCD__)
2477 .................... float48 sinh(float48 x) 2497 .................... float48 sinh(float48 x)
2478 .................... { 2498 .................... {
2479 .................... 2499 ....................
2480 .................... return ((exp(x) - exp(-x))/2); 2500 .................... return ((exp(x) - exp(-x))/2);
2481 .................... } 2501 .................... }
2482 .................... 2502 ....................
2483 .................... // Overloaded function sinh() for data type - Float48 2503 .................... // Overloaded function sinh() for data type - Float48
2484 .................... float64 sinh(float64 x) 2504 .................... float64 sinh(float64 x)
2485 .................... { 2505 .................... {
2486 .................... 2506 ....................
2487 .................... return ((exp(x) - exp(-x))/2); 2507 .................... return ((exp(x) - exp(-x))/2);
2488 .................... } 2508 .................... }
2489 .................... #endif 2509 .................... #endif
2490 .................... 2510 ....................
2491 .................... //////////////////////////////////////////////////////////////////////////// 2511 .................... ////////////////////////////////////////////////////////////////////////////
2492 .................... // float tanh(float x) 2512 .................... // float tanh(float x)
2493 .................... //////////////////////////////////////////////////////////////////////////// 2513 .................... ////////////////////////////////////////////////////////////////////////////
2494 .................... // Description : Computes the hyperbolic tangent value of x 2514 .................... // Description : Computes the hyperbolic tangent value of x
2495 .................... // Returns : returns the hyperbolic tangent value of x 2515 .................... // Returns : returns the hyperbolic tangent value of x
2496 .................... // Date : N/A 2516 .................... // Date : N/A
2497 .................... // 2517 .................... //
2498 .................... 2518 ....................
2499 .................... float32 tanh(float32 x) 2519 .................... float32 tanh(float32 x)
2500 .................... { 2520 .................... {
2501 .................... return(sinh(x)/cosh(x)); 2521 .................... return(sinh(x)/cosh(x));
2502 .................... } 2522 .................... }
2503 .................... //Overloaded functions for tanh() for PCD 2523 .................... //Overloaded functions for tanh() for PCD
2504 .................... // Overloaded function tanh() for data type - Float48 2524 .................... // Overloaded function tanh() for data type - Float48
2505 .................... #if defined(__PCD__) 2525 .................... #if defined(__PCD__)
2506 .................... float48 tanh(float48 x) 2526 .................... float48 tanh(float48 x)
2507 .................... { 2527 .................... {
2508 .................... return(sinh(x)/cosh(x)); 2528 .................... return(sinh(x)/cosh(x));
2509 .................... } 2529 .................... }
2510 .................... 2530 ....................
2511 .................... // Overloaded function tanh() for data type - Float64 2531 .................... // Overloaded function tanh() for data type - Float64
2512 .................... float64 tanh(float64 x) 2532 .................... float64 tanh(float64 x)
2513 .................... { 2533 .................... {
2514 .................... return(sinh(x)/cosh(x)); 2534 .................... return(sinh(x)/cosh(x));
2515 .................... } 2535 .................... }
2516 .................... #endif 2536 .................... #endif
2517 .................... 2537 ....................
2518 .................... //////////////////////////////////////////////////////////////////////////// 2538 .................... ////////////////////////////////////////////////////////////////////////////
2519 .................... // float frexp(float x, signed int *exp) 2539 .................... // float frexp(float x, signed int *exp)
2520 .................... //////////////////////////////////////////////////////////////////////////// 2540 .................... ////////////////////////////////////////////////////////////////////////////
2521 .................... // Description : breaks a floating point number into a normalized fraction and an integral 2541 .................... // 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. 2542 .................... // 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 2543 .................... // 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, 2544 .................... // [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. 2545 .................... // both parts of the result are zero.
2526 .................... // Date : N/A 2546 .................... // Date : N/A
2527 .................... // 2547 .................... //
2528 .................... 2548 ....................
2529 .................... #define LOG2 .30102999566398119521 2549 .................... #define LOG2 .30102999566398119521
2530 .................... float32 frexp(float32 x, signed int8 *exp) 2550 .................... float32 frexp(float32 x, signed int8 *exp)
2531 .................... { 2551 .................... {
2532 .................... float32 res; 2552 .................... float32 res;
2533 .................... int1 sign = 0; 2553 .................... int1 sign = 0;
2534 .................... if(x == 0.0) 2554 .................... if(x == 0.0)
2535 .................... { 2555 .................... {
2536 .................... *exp=0; 2556 .................... *exp=0;
2537 .................... return (0.0); 2557 .................... return (0.0);
2538 .................... } 2558 .................... }
2539 .................... if(x < 0.0) 2559 .................... if(x < 0.0)
2540 .................... { 2560 .................... {
2541 .................... x=-x; 2561 .................... x=-x;
2542 .................... sign=1; 2562 .................... sign=1;
2543 .................... } 2563 .................... }
2544 .................... if (x > 1.0) 2564 .................... if (x > 1.0)
2545 .................... { 2565 .................... {
2546 .................... *exp=(ceil(log10(x)/LOG2)); 2566 .................... *exp=(ceil(log10(x)/LOG2));
2547 .................... res=x/(pow(2, *exp)); 2567 .................... res=x/(pow(2, *exp));
2548 .................... if (res == 1) 2568 .................... if (res == 1)
2549 .................... { 2569 .................... {
2550 .................... *exp=*exp+1; 2570 .................... *exp=*exp+1;
2551 .................... res=.5; 2571 .................... res=.5;
2552 .................... } 2572 .................... }
2553 .................... } 2573 .................... }
2554 .................... else 2574 .................... else
2555 .................... { 2575 .................... {
2556 .................... if(x < 0.5) 2576 .................... if(x < 0.5)
2557 .................... { 2577 .................... {
2558 .................... *exp=-1; 2578 .................... *exp=-1;
2559 .................... res=x*2; 2579 .................... res=x*2;
2560 .................... } 2580 .................... }
2561 .................... else 2581 .................... else
2562 .................... { 2582 .................... {
2563 .................... *exp=0; 2583 .................... *exp=0;
2564 .................... res=x; 2584 .................... res=x;
2565 .................... } 2585 .................... }
2566 .................... } 2586 .................... }
2567 .................... if(sign) 2587 .................... if(sign)
2568 .................... { 2588 .................... {
2569 .................... res=-res; 2589 .................... res=-res;
2570 .................... } 2590 .................... }
2571 .................... return res; 2591 .................... return res;
2572 .................... } 2592 .................... }
2573 .................... 2593 ....................
2574 .................... //Overloaded functions for frexp() for PCD 2594 .................... //Overloaded functions for frexp() for PCD
2575 .................... // Overloaded function frexp() for data type - Float48 2595 .................... // Overloaded function frexp() for data type - Float48
2576 .................... #if defined(__PCD__) 2596 .................... #if defined(__PCD__)
2577 .................... float48 frexp(float48 x, signed int8 *exp) 2597 .................... float48 frexp(float48 x, signed int8 *exp)
2578 .................... { 2598 .................... {
2579 .................... float48 res; 2599 .................... float48 res;
2580 .................... int1 sign = 0; 2600 .................... int1 sign = 0;
2581 .................... if(x == 0.0) 2601 .................... if(x == 0.0)
2582 .................... { 2602 .................... {
2583 .................... *exp=0; 2603 .................... *exp=0;
2584 .................... return (0.0); 2604 .................... return (0.0);
2585 .................... } 2605 .................... }
2586 .................... if(x < 0.0) 2606 .................... if(x < 0.0)
2587 .................... { 2607 .................... {
2588 .................... x=-x; 2608 .................... x=-x;
2589 .................... sign=1; 2609 .................... sign=1;
2590 .................... } 2610 .................... }
2591 .................... if (x > 1.0) 2611 .................... if (x > 1.0)
2592 .................... { 2612 .................... {
2593 .................... *exp=(ceil(log10(x)/LOG2)); 2613 .................... *exp=(ceil(log10(x)/LOG2));
2594 .................... res=x/(pow(2, *exp)); 2614 .................... res=x/(pow(2, *exp));
2595 .................... if (res == 1) 2615 .................... if (res == 1)
2596 .................... { 2616 .................... {
2597 .................... *exp=*exp+1; 2617 .................... *exp=*exp+1;
2598 .................... res=.5; 2618 .................... res=.5;
2599 .................... } 2619 .................... }
2600 .................... } 2620 .................... }
2601 .................... else 2621 .................... else
2602 .................... { 2622 .................... {
2603 .................... if(x < 0.5) 2623 .................... if(x < 0.5)
2604 .................... { 2624 .................... {
2605 .................... *exp=-1; 2625 .................... *exp=-1;
2606 .................... res=x*2; 2626 .................... res=x*2;
2607 .................... } 2627 .................... }
2608 .................... else 2628 .................... else
2609 .................... { 2629 .................... {
2610 .................... *exp=0; 2630 .................... *exp=0;
2611 .................... res=x; 2631 .................... res=x;
2612 .................... } 2632 .................... }
2613 .................... } 2633 .................... }
2614 .................... if(sign) 2634 .................... if(sign)
2615 .................... { 2635 .................... {
2616 .................... res=-res; 2636 .................... res=-res;
2617 .................... } 2637 .................... }
2618 .................... return res; 2638 .................... return res;
2619 .................... } 2639 .................... }
2620 .................... 2640 ....................
2621 .................... // Overloaded function frexp() for data type - Float64 2641 .................... // Overloaded function frexp() for data type - Float64
2622 .................... float64 frexp(float64 x, signed int8 *exp) 2642 .................... float64 frexp(float64 x, signed int8 *exp)
2623 .................... { 2643 .................... {
2624 .................... float64 res; 2644 .................... float64 res;
2625 .................... int1 sign = 0; 2645 .................... int1 sign = 0;
2626 .................... if(x == 0.0) 2646 .................... if(x == 0.0)
2627 .................... { 2647 .................... {
2628 .................... *exp=0; 2648 .................... *exp=0;
2629 .................... return (0.0); 2649 .................... return (0.0);
2630 .................... } 2650 .................... }
2631 .................... if(x < 0.0) 2651 .................... if(x < 0.0)
2632 .................... { 2652 .................... {
2633 .................... x=-x; 2653 .................... x=-x;
2634 .................... sign=1; 2654 .................... sign=1;
2635 .................... } 2655 .................... }
2636 .................... if (x > 1.0) 2656 .................... if (x > 1.0)
2637 .................... { 2657 .................... {
2638 .................... *exp=(ceil(log10(x)/LOG2)); 2658 .................... *exp=(ceil(log10(x)/LOG2));
2639 .................... res=x/(pow(2, *exp)); 2659 .................... res=x/(pow(2, *exp));
2640 .................... if (res == 1) 2660 .................... if (res == 1)
2641 .................... { 2661 .................... {
2642 .................... *exp=*exp+1; 2662 .................... *exp=*exp+1;
2643 .................... res=.5; 2663 .................... res=.5;
2644 .................... } 2664 .................... }
2645 .................... } 2665 .................... }
2646 .................... else 2666 .................... else
2647 .................... { 2667 .................... {
2648 .................... if(x < 0.5) 2668 .................... if(x < 0.5)
2649 .................... { 2669 .................... {
2650 .................... *exp=-1; 2670 .................... *exp=-1;
2651 .................... res=x*2; 2671 .................... res=x*2;
2652 .................... } 2672 .................... }
2653 .................... else 2673 .................... else
2654 .................... { 2674 .................... {
2655 .................... *exp=0; 2675 .................... *exp=0;
2656 .................... res=x; 2676 .................... res=x;
2657 .................... } 2677 .................... }
2658 .................... } 2678 .................... }
2659 .................... if(sign) 2679 .................... if(sign)
2660 .................... { 2680 .................... {
2661 .................... res=-res; 2681 .................... res=-res;
2662 .................... } 2682 .................... }
2663 .................... return res; 2683 .................... return res;
2664 .................... } 2684 .................... }
2665 .................... #endif 2685 .................... #endif
2666 .................... 2686 ....................
2667 .................... ////////////////////////////////////////////////////////////////////////////// 2687 .................... //////////////////////////////////////////////////////////////////////////////
2668 .................... // float ldexp(float x, signed int *exp) 2688 .................... // float ldexp(float x, signed int *exp)
2669 .................... ////////////////////////////////////////////////////////////////////////////// 2689 .................... //////////////////////////////////////////////////////////////////////////////
2670 .................... // Description : multiplies a floating point number by an integral power of 2. 2690 .................... // 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. 2691 .................... // Returns : returns the value of x times 2 raised to the power exp.
2672 .................... // Date : N/A 2692 .................... // Date : N/A
2673 .................... // 2693 .................... //
2674 .................... 2694 ....................
2675 .................... float32 ldexp(float32 value, signed int8 exp) 2695 .................... float32 ldexp(float32 value, signed int8 exp)
2676 .................... { 2696 .................... {
2677 .................... return (value * pow(2,exp)); 2697 .................... return (value * pow(2,exp));
2678 .................... } 2698 .................... }
2679 .................... //Overloaded functions for ldexp() for PCD 2699 .................... //Overloaded functions for ldexp() for PCD
2680 .................... // Overloaded function ldexp() for data type - Float48 2700 .................... // Overloaded function ldexp() for data type - Float48
2681 .................... 2701 ....................
2682 .................... #if defined(__PCD__) 2702 .................... #if defined(__PCD__)
2683 .................... float48 ldexp(float48 value, signed int8 exp) 2703 .................... float48 ldexp(float48 value, signed int8 exp)
2684 .................... { 2704 .................... {
2685 .................... return (value * pow(2,exp)); 2705 .................... return (value * pow(2,exp));
2686 .................... } 2706 .................... }
2687 .................... // Overloaded function ldexp() for data type - Float64 2707 .................... // Overloaded function ldexp() for data type - Float64
2688 .................... float64 ldexp(float64 value, signed int8 exp) 2708 .................... float64 ldexp(float64 value, signed int8 exp)
2689 .................... { 2709 .................... {
2690 .................... return (value * pow(2,exp)); 2710 .................... return (value * pow(2,exp));
2691 .................... } 2711 .................... }
2692 .................... #endif 2712 .................... #endif
2693 .................... 2713 ....................
2694 .................... #endif 2714 .................... #endif
2695 .................... 2715 ....................
2696 .................... 2716 ....................
2697 .................... void main() 2717 .................... void main()
2698 .................... { 2718 .................... {
2699 * 2719 *
2700 024F: CLRF 04 2720 0266: CLRF 04
2701 0250: BCF 03.7 2721 0267: BCF 03.7
2702 0251: MOVLW 1F 2722 0268: MOVLW 1F
2703 0252: ANDWF 03,F 2723 0269: ANDWF 03,F
2704 0253: MOVLW 71 2724 026A: MOVLW 71
2705 0254: BSF 03.5 2725 026B: BSF 03.5
2706 0255: MOVWF 0F 2726 026C: MOVWF 0F
2707 0256: MOVF 0F,W 2727 026D: MOVF 0F,W
2708 0257: BCF 03.5 2728 026E: BCF 03.5
2709 0258: BCF 20.7 2729 026F: BCF 20.7
2710 0259: MOVF 20,W 2730 0270: MOVF 20,W
2711 025A: BSF 03.5 2731 0271: BSF 03.5
2712 025B: MOVWF 07 2732 0272: MOVWF 07
2713 025C: BCF 03.5 2733 0273: BCF 03.5
2714 025D: BSF 07.7 2734 0274: BSF 07.7
2715 025E: BSF 03.5 2735 0275: BSF 03.5
2716 025F: BSF 03.6 2736 0276: BSF 03.6
2717 0260: MOVF 09,W 2737 0277: MOVF 09,W
2718 0261: ANDLW C0 2738 0278: ANDLW C0
2719 0262: MOVWF 09 2739 0279: MOVWF 09
2720 0263: BCF 03.6 2740 027A: BCF 03.6
2721 0264: BCF 1F.4 2741 027B: BCF 1F.4
2722 0265: BCF 1F.5 2742 027C: BCF 1F.5
2723 0266: MOVLW 00 2743 027D: MOVLW 00
2724 0267: BSF 03.6 2744 027E: BSF 03.6
2725 0268: MOVWF 08 2745 027F: MOVWF 08
2726 0269: BCF 03.5 2746 0280: BCF 03.5
2727 026A: CLRF 07 2747 0281: CLRF 07
2728 026B: CLRF 08 2748 0282: CLRF 08
2729 026C: CLRF 09 2749 0283: CLRF 09
2730 * 2750 *
2731 0276: CLRF 34 2751 028D: CLRF 34
2732 0277: CLRF 33 2752 028E: CLRF 33
2733 .................... float last,b,anemo_speed; 2753 .................... float last,b,anemo_speed;
2734 .................... unsigned int16 anemo_round=0; 2754 .................... unsigned int16 anemo_round=0;
2735 .................... unsigned int16 i; 2755 .................... unsigned int16 i;
2736 .................... 2756 ....................
2737 .................... -  
2738 .................... //signed int16 X,Y,Z; 2757 .................... //signed int16 X,Y,Z;
2739 .................... setup_adc_ports(NO_ANALOGS|VSS_VDD); 2758 .................... setup_adc_ports(NO_ANALOGS|VSS_VDD);
2740 0278: BSF 03.5 2759 028F: BSF 03.5
2741 0279: BSF 03.6 2760 0290: BSF 03.6
2742 027A: MOVF 09,W 2761 0291: MOVF 09,W
2743 027B: ANDLW C0 2762 0292: ANDLW C0
2744 027C: MOVWF 09 2763 0293: MOVWF 09
2745 027D: BCF 03.6 2764 0294: BCF 03.6
2746 027E: BCF 1F.4 2765 0295: BCF 1F.4
2747 027F: BCF 1F.5 2766 0296: BCF 1F.5
2748 0280: MOVLW 00 2767 0297: MOVLW 00
2749 0281: BSF 03.6 2768 0298: BSF 03.6
2750 0282: MOVWF 08 2769 0299: MOVWF 08
2751 .................... setup_adc(ADC_CLOCK_DIV_2); 2770 .................... setup_adc(ADC_CLOCK_DIV_2);
2752 0283: BCF 03.5 2771 029A: BCF 03.5
2753 0284: BCF 03.6 2772 029B: BCF 03.6
2754 0285: BCF 1F.6 2773 029C: BCF 1F.6
2755 0286: BCF 1F.7 -  
2756 0287: BSF 03.5 -  
2757 0288: BCF 1F.7 2774 029D: BCF 1F.7
2758 0289: BCF 03.5 -  
2759 028A: BSF 1F.0 -  
2760 .................... setup_spi(SPI_SS_DISABLED); -  
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 2775 029E: BSF 03.5
2781 029F: MOVWF 14 2776 029F: BCF 1F.7
2782 .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); -  
2783 02A0: MOVF 01,W 2777 02A0: BCF 03.5
2784 02A1: ANDLW C7 2778 02A1: BSF 1F.0
-   2779 .................... setup_spi(SPI_SS_DISABLED);
2785 02A2: IORLW 08 2780 02A2: BCF 14.5
2786 02A3: MOVWF 01 2781 02A3: BCF 20.5
2787 .................... setup_timer_1(T1_DISABLED); -  
2788 02A4: BCF 03.5 2782 02A4: MOVF 20,W
2789 02A5: CLRF 10 2783 02A5: BSF 03.5
2790 .................... setup_timer_2(T2_DISABLED,0,1); -  
2791 02A6: MOVLW 00 2784 02A6: MOVWF 07
2792 02A7: MOVWF 78 2785 02A7: BCF 03.5
2793 02A8: MOVWF 12 2786 02A8: BSF 20.4
2794 02A9: MOVLW 00 2787 02A9: MOVF 20,W
2795 02AA: BSF 03.5 2788 02AA: BSF 03.5
2796 02AB: MOVWF 12 2789 02AB: MOVWF 07
2797 .................... setup_ccp1(CCP_OFF); -  
2798 02AC: BCF 03.5 2790 02AC: BCF 03.5
2799 02AD: BSF 20.2 2791 02AD: BCF 20.3
2800 02AE: MOVF 20,W 2792 02AE: MOVF 20,W
2801 02AF: BSF 03.5 2793 02AF: BSF 03.5
2802 02B0: MOVWF 07 2794 02B0: MOVWF 07
-   2795 02B1: MOVLW 01
2803 02B1: BCF 03.5 2796 02B2: BCF 03.5
-   2797 02B3: MOVWF 14
-   2798 02B4: MOVLW 00
-   2799 02B5: BSF 03.5
-   2800 02B6: MOVWF 14
-   2801 .................... setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
-   2802 02B7: MOVF 01,W
-   2803 02B8: ANDLW C7
-   2804 02B9: IORLW 08
-   2805 02BA: MOVWF 01
-   2806 .................... setup_timer_1(T1_DISABLED);
-   2807 02BB: BCF 03.5
-   2808 02BC: CLRF 10
-   2809 .................... setup_timer_2(T2_DISABLED,0,1);
-   2810 02BD: MOVLW 00
-   2811 02BE: MOVWF 78
-   2812 02BF: MOVWF 12
-   2813 02C0: MOVLW 00
-   2814 02C1: BSF 03.5
-   2815 02C2: MOVWF 12
-   2816 .................... setup_ccp1(CCP_OFF);
-   2817 02C3: BCF 03.5
-   2818 02C4: BSF 20.2
-   2819 02C5: MOVF 20,W
-   2820 02C6: BSF 03.5
-   2821 02C7: MOVWF 07
-   2822 02C8: BCF 03.5
2804 02B2: CLRF 17 2823 02C9: CLRF 17
2805 02B3: BSF 03.5 2824 02CA: BSF 03.5
2806 02B4: CLRF 1B 2825 02CB: CLRF 1B
2807 02B5: CLRF 1C 2826 02CC: CLRF 1C
2808 02B6: MOVLW 01 2827 02CD: MOVLW 01
2809 02B7: MOVWF 1D 2828 02CE: MOVWF 1D
2810 .................... setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard 2829 .................... setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard
2811 02B8: BCF 03.5 2830 02CF: BCF 03.5
2812 02B9: BSF 03.6 2831 02D0: BSF 03.6
2813 02BA: CLRF 07 2832 02D1: CLRF 07
2814 02BB: CLRF 08 2833 02D2: CLRF 08
2815 02BC: CLRF 09 2834 02D3: CLRF 09
-   2835 ....................
-   2836 .................... printf("Magnetometr: \r\n",);
-   2837 02D4: MOVLW 04
-   2838 02D5: MOVWF 0D
-   2839 02D6: MOVLW 00
-   2840 02D7: MOVWF 0F
-   2841 02D8: BCF 03.6
-   2842 02D9: CALL 043
-   2843 .................... printf("(c)mlab JACHO 2013: \r\n",);
-   2844 02DA: MOVLW 0D
-   2845 02DB: BSF 03.6
-   2846 02DC: MOVWF 0D
-   2847 02DD: MOVLW 00
-   2848 02DE: MOVWF 0F
-   2849 02DF: BCF 03.6
-   2850 02E0: CALL 043
-   2851 .................... printf("X, Y, Z \r\n",);
-   2852 02E1: MOVLW 19
-   2853 02E2: BSF 03.6
-   2854 02E3: MOVWF 0D
-   2855 02E4: MOVLW 00
-   2856 02E5: MOVWF 0F
-   2857 02E6: BCF 03.6
-   2858 02E7: CALL 043
-   2859 ....................
2816 .................... 2860 ....................
2817 .................... // Init the HMC5883L. Set Mode register for 2861 .................... // Init the HMC5883L. Set Mode register for
2818 .................... // continuous measurements. 2862 .................... // continuous measurements.
2819 .................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18); // no average, maximal update range 2863 .................... hmc5883l_write_reg(HMC5883L_CFG_A_REG, 0x18); // no average, maximal update range
2820 02BD: BCF 03.6 -  
2821 02BE: CLRF 37 2864 02E8: CLRF 37
2822 02BF: MOVLW 18 2865 02E9: MOVLW 18
2823 02C0: MOVWF 38 2866 02EA: MOVWF 38
2824 02C1: CALL 069 2867 02EB: CALL 0D3
2825 .................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0xE0); // maximal range 2868 .................... hmc5883l_write_reg(HMC5883L_CFG_B_REG, 0xE0); // maximal range
2826 02C2: MOVLW 01 2869 02EC: MOVLW 01
2827 02C3: MOVWF 37 2870 02ED: MOVWF 37
2828 02C4: MOVLW E0 2871 02EE: MOVLW E0
2829 02C5: MOVWF 38 2872 02EF: MOVWF 38
2830 02C6: CALL 069 2873 02F0: CALL 0D3
2831 .................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00); 2874 .................... hmc5883l_write_reg(HMC5883L_MODE_REG, 0x00);
2832 02C7: MOVLW 02 2875 02F1: MOVLW 02
2833 02C8: MOVWF 37 2876 02F2: MOVWF 37
2834 02C9: CLRF 38 2877 02F3: CLRF 38
2835 02CA: CALL 069 2878 02F4: CALL 0D3
2836 .................... 2879 ....................
2837 .................... // Continuously read and display the x,y,z results. 2880 .................... // Continuously read and display the x,y,z results.
2838 .................... // Wait at least 67 ms between reads, re the HMC5883L data sheet. 2881 .................... // Wait at least 67 ms between reads, re the HMC5883L data sheet.
2839 .................... 2882 ....................
2840 .................... 2883 ....................
2841 .................... printf("Magnetometr: \r\n",); -  
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 -  
2849 .................... printf("(c)mlab JACHO 2013: \r\n",); -  
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 -  
2857 .................... printf("X, Y, Z \r\n",); -  
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 -  
2865 .................... -  
2866 .................... /* while(true) -  
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); -  
2873 .................... -  
2874 .................... } -  
2875 .................... */ -  
2876 .................... -  
2877 .................... while(TRUE) 2884 .................... while(TRUE)
2878 .................... { 2885 .................... {
2879 .................... 2886 ....................
2880 .................... // for(i=0;i<=10;i++) -  
2881 .................... // { -  
2882 .................... hmc5883l_read_data(); 2887 .................... hmc5883l_read_data();
2883 02E0: GOTO 150 2888 02F5: GOTO 151
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; -  
2888 .................... -  
2889 .................... // anemo_speed += (b-last); -  
2890 .................... // last=b; -  
2891 .................... // delay_ms(10); -  
2892 .................... // } -  
2893 .................... // anemo_speed=anemo_speed/10; -  
2894 .................... -  
2895 .................... printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z); 2889 .................... printf("%6Ld %6Ld %6Ld \n\r", compass.x, compass.y, compass.z);
2896 02E1: MOVLW 00 2890 02F6: MOVLW 00
2897 02E2: MOVWF 04 2891 02F7: MOVWF 04
2898 02E3: MOVF 22,W 2892 02F8: MOVF 22,W
2899 02E4: MOVWF 38 2893 02F9: MOVWF 38
2900 02E5: MOVF 21,W 2894 02FA: MOVF 21,W
2901 02E6: MOVWF 37 2895 02FB: MOVWF 37
2902 02E7: CALL 1CF 2896 02FC: CALL 1D0
2903 02E8: MOVLW 20 2897 02FD: MOVLW 20
2904 02E9: MOVWF 40 2898 02FE: MOVWF 40
2905 02EA: CALL 0A2 2899 02FF: CALL 01F
2906 02EB: MOVLW 00 2900 0300: MOVLW 00
2907 02EC: MOVWF 04 2901 0301: MOVWF 04
2908 02ED: MOVF 24,W 2902 0302: MOVF 24,W
2909 02EE: MOVWF 38 2903 0303: MOVWF 38
2910 02EF: MOVF 23,W 2904 0304: MOVF 23,W
2911 02F0: MOVWF 37 2905 0305: MOVWF 37
2912 02F1: CALL 1CF 2906 0306: CALL 1D0
2913 02F2: MOVLW 20 2907 0307: MOVLW 20
2914 02F3: MOVWF 40 2908 0308: MOVWF 40
2915 02F4: CALL 0A2 2909 0309: CALL 01F
2916 02F5: MOVLW 00 2910 030A: MOVLW 00
2917 02F6: MOVWF 04 2911 030B: MOVWF 04
2918 02F7: MOVF 26,W 2912 030C: MOVF 26,W
2919 02F8: MOVWF 38 2913 030D: MOVWF 38
2920 02F9: MOVF 25,W 2914 030E: MOVF 25,W
2921 02FA: MOVWF 37 2915 030F: MOVWF 37
2922 02FB: CALL 1CF 2916 0310: CALL 1D0
2923 02FC: MOVLW 20 2917 0311: MOVLW 20
2924 02FD: MOVWF 40 2918 0312: MOVWF 40
2925 02FE: CALL 0A2 2919 0313: CALL 01F
2926 02FF: MOVLW 0A 2920 0314: MOVLW 0A
2927 0300: MOVWF 40 2921 0315: MOVWF 40
2928 0301: CALL 0A2 2922 0316: CALL 01F
2929 0302: MOVLW 0D 2923 0317: MOVLW 0D
2930 0303: MOVWF 40 2924 0318: MOVWF 40
2931 0304: CALL 0A2 2925 0319: CALL 01F
2932 .................... // delay_ms(100); 2926 .................... delay_ms(100);
-   2927 031A: MOVLW 64
-   2928 031B: MOVWF 37
-   2929 031C: GOTO 250
2933 .................... } 2930 .................... }
2934 0305: GOTO 2E0 2931 031D: GOTO 2F5
2935 .................... 2932 ....................
2936 .................... } 2933 .................... }
2937 .................... 2934 ....................
2938 .................... 2935 ....................
2939 .................... 2936 ....................
2940 0306: SLEEP 2937 031E: SLEEP
2941   2938  
2942 Configuration Fuses: 2939 Configuration Fuses:
2943 Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG 2940 Word 1: 2CF5 INTRC NOWDT NOPUT MCLR NOPROTECT NOCPD NOBROWNOUT IESO FCMEN NOLVP NODEBUG
2944 Word 2: 3FFF NOWRT BORV40 2941 Word 2: 3FFF NOWRT BORV40