Mercurial > public > mk2
annotate code_part1/OSTC_code_asm_part1/math.asm @ 363:93a64a19728e 64kByte Logbook
small typo...
author | Heinrichsweikamp |
---|---|
date | Wed, 08 Jun 2011 19:55:16 +0200 |
parents | cec312042b94 |
children | 07f5b0baaa57 |
rev | line source |
---|---|
0 | 1 ; OSTC - diving computer code |
2 ; Copyright (C) 2008 HeinrichsWeikamp GbR | |
3 | |
4 ; This program is free software: you can redistribute it and/or modify | |
5 ; it under the terms of the GNU General Public License as published by | |
6 ; the Free Software Foundation, either version 3 of the License, or | |
7 ; (at your option) any later version. | |
8 | |
9 ; This program is distributed in the hope that it will be useful, | |
10 ; but WITHOUT ANY WARRANTY; without even the implied warranty of | |
11 ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
12 ; GNU General Public License for more details. | |
13 | |
14 ; You should have received a copy of the GNU General Public License | |
15 ; along with this program. If not, see <http://www.gnu.org/licenses/>. | |
16 | |
17 | |
18 ; Math routines | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
19 ; history: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
20 ; 2005-10-30: Written by Matthias Heinrichs, info@heinrichsweikamp.com |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
21 ; 2007-06-21: MH last updated |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
22 ; 2011-01-19: Clean up of isr variante. |
0 | 23 ; known bugs: |
24 ; ToDo: clean up! | |
25 | |
88 | 26 |
0 | 27 convert_time: ; converts hi:lo in minutes to hours (hi) and minutes (lo) |
28 movff lo,xA+0 ; divide by 60... | |
29 movff hi,xA+1 ; | |
30 movlw d'60' ; | |
31 movwf xB+0 ; | |
32 clrf xB+1 ; | |
33 rcall div16x16 ; xA/xB=xC with xA as remainder | |
34 movff xC+0,hi ; Hours | |
35 movff xA+0,lo ; =remaining minutes (0.....59) | |
36 return | |
37 | |
38 div16: | |
39 ; divA=divA/2^divB (divB: 8Bit only!) | |
40 bcf STATUS,C | |
41 rrcf divA+1 | |
42 rrcf divA | |
43 decfsz divB | |
44 bra div16 | |
45 return | |
46 | |
47 sub16: | |
48 ; sub_c = sub_a - sub_b | |
49 bcf neg_flag | |
74 | 50 movf sub_b+0, W ; Get Value to be subtracted |
51 subwf sub_a+0, W ; Do the High Byte | |
52 movwf sub_c+0 | |
53 movf sub_b+1, W ; Get the Value to be Subbed | |
54 subwfb sub_a+1, W | |
55 movwf sub_c+1 | |
342
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
56 |
354 | 57 btfss STATUS,N ; Negativ result ? |
58 return ; NO: result positive done. | |
342
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
59 |
354 | 60 bsf neg_flag ; MARK result negative |
342
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
61 |
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
62 comf sub_c+1 ; 16bit sign change. |
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
63 negf sub_c+0 |
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
64 btfsc STATUS,C ; Carry to propagate ? |
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
65 incf sub_c+1,F ; YES: do it. |
06299199dfb9
Fix bad max depth value (fix sub16 to avoid trashing sub_b).
JeanDo
parents:
161
diff
changeset
|
66 |
0 | 67 return |
68 | |
105 | 69 mult16x16: ;xA*xB=xC |
0 | 70 clrf xC+2 ; Clear the High-Order Bits |
71 clrf xC+3 | |
72 movf xA, w ; Do the "L" Multiplication first | |
73 mulwf xB | |
74 movf PRODL, w ; Save result | |
75 movwf xC | |
76 movf PRODH, w | |
77 movwf xC+1 | |
78 movf xA, w ; Do the "I" Multiplication | |
79 mulwf xB+1 | |
80 movf PRODL, w ; Save the Most Significant Byte First | |
81 addwf xC+1, f | |
82 movf PRODH, w | |
83 addwfc xC+2, f ; Add to the Last Result | |
84 movf xA+1, w ; Do the "O" Multiplication | |
85 mulwf xB | |
86 movf PRODL, w ; Add the Lower Byte Next | |
87 addwf xC+1, f | |
88 movf PRODH, w ; Add the High Byte First | |
89 addwfc xC+2, f | |
90 btfsc STATUS, C ; Add the Carry | |
91 incf xC+3, f | |
92 movf xA+1, w ; Do the "F" Multiplication | |
93 mulwf xB+1 | |
94 movf PRODL, w | |
95 addwf xC+2, f | |
96 movf PRODH, w | |
97 addwfc xC+3, f | |
98 return | |
99 | |
100 | |
101 div16x16: ;xA/xB=xC with xA as remainder | |
102 ;uses divB as temp variable | |
103 clrf xC+0 | |
104 clrf xC+1 | |
105 MOVF xB+0,W ; Check for zero | |
106 IORWF xB+1,W ; | |
107 BTFSC STATUS,Z ; Check for zero | |
108 RETLW H'FF' ; return 0xFF if illegal | |
109 MOVLW 1 ; Start count at 1 | |
110 MOVWF divB ; Clear Count | |
111 div16x16_1 | |
112 BTFSC xB+1,7 ; High bit set ? | |
113 bra div16x16_2 ; Yes then continue | |
114 INCF divB,F ; Increment count | |
115 | |
116 bcf STATUS,C | |
117 rlcf xB+0,F | |
118 rlcf xB+1,F | |
119 bra div16x16_1 | |
120 div16x16_2: | |
121 ; Shift result left | |
122 bcf STATUS,C | |
123 rlcf xC+0,F | |
124 rlcf xC+1,F | |
125 | |
126 ; Reduce Divisor | |
127 | |
128 MOVF xB,W ; Get low byte of subtrahend | |
129 SUBWF xA,F ; Subtract DST(low) - SRC(low) | |
130 MOVF xB+1,W ; Now get high byte of subtrahend | |
131 BTFSS STATUS,C ; If there was a borrow, rather than | |
132 INCF xB+1,W ; decrement high byte of dst we inc src | |
133 SUBWF xA+1,F ; Subtract the high byte and we're done | |
134 | |
135 | |
136 BTFSC STATUS, C ; Did it reduce? | |
137 bra div16x16_3 ; No, so it was less than | |
138 | |
139 movf xB+0,W ; Reverse subtraction | |
140 addwf xA+0,F | |
141 movf xB+1,W | |
142 addwfc xA+1,F | |
143 | |
144 bra div16x16_4 ; Continue the process | |
145 div16x16_3: | |
146 BSF xC+0,0 ; Yes it did, this gets a 1 bit | |
147 div16x16_4: | |
148 DECF divB,F ; Decrement N_COUNT | |
149 BTFSC STATUS,Z ; If its not zero then continue | |
150 return | |
151 | |
152 bcf STATUS,C | |
153 rrcf xB+1,F | |
154 rrcf xB+0,F | |
155 | |
156 bra div16x16_2 ; Next bit. | |
157 | |
158 div32x16: ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder | |
159 ; Setup | |
160 movlw .32 ; setup shift counter | |
161 movwf divB | |
162 movf xC+3,W ; move ACCb to ACCf | |
163 movwf xA+1 | |
164 movf xC+2,W | |
165 movwf xA+0 | |
166 movf xC+1,W ; move ACCc to ACCe | |
167 movwf sub_a+1 | |
168 movf xC+0,W | |
169 movwf sub_a+0 | |
170 clrf xC+3 | |
171 clrf xC+2 | |
172 clrf xC+1 | |
173 clrf xC+0 | |
174 clrf sub_b+1 | |
175 clrf sub_b+0 | |
176 div32x16_2 | |
177 bcf STATUS,C | |
178 rlcf sub_a+0,F | |
179 rlcf sub_a+1,F | |
180 rlcf xA+0,F | |
181 rlcf xA+1,F | |
182 rlcf sub_b+0,F | |
183 rlcf sub_b+1,F | |
184 movf xB+1,W | |
185 subwf sub_b+1,W ; check if a>d | |
186 btfss STATUS,Z | |
187 goto div32x16_3 | |
188 movf xB+0,W | |
189 subwf sub_b+0,W ; if msb equal then check lsb | |
190 div32x16_3 | |
191 btfss STATUS,C ; carry set if d>a | |
192 goto div32x16_4 | |
193 movf xB+0,W ; d-a into d | |
194 subwf sub_b+0,F | |
195 btfss STATUS,C | |
196 decf sub_b+1,F | |
197 movf xB+1,W | |
198 subwf sub_b+1,F | |
199 bsf STATUS,C ; shift a 1 into b (result) | |
200 div32x16_4 | |
201 rlcf xC+0,F | |
202 rlcf xC+1,F | |
203 rlcf xC+2,F | |
204 rlcf xC+3,F | |
205 decfsz divB,F ; loop until all bits checked | |
206 goto div32x16_2 | |
207 return | |
208 | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
209 ;============================================================================= |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
210 ; u16 * u16 --> 32bit multiply (xA * xB --> xC) |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
211 ; Used in interupt service routines, to compute temperature and pressure. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
212 ; |
0 | 213 isr_mult16x16: |
214 clrf isr_xC+2 ; Clear the High-Order Bits | |
215 clrf isr_xC+3 | |
216 movf isr_xA, w ; Do the "L" Multiplication first | |
217 mulwf isr_xB | |
218 movf PRODL, w ; Save result | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
219 movwf isr_xC+0 |
0 | 220 movf PRODH, w |
221 movwf isr_xC+1 | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
222 movf isr_xA+0, w ; Do the "I" Multiplication |
0 | 223 mulwf isr_xB+1 |
224 movf PRODL, w ; Save the Most Significant Byte First | |
225 addwf isr_xC+1, f | |
226 movf PRODH, w | |
227 addwfc isr_xC+2, f ; Add to the Last Result | |
228 movf isr_xA+1, w ; Do the "O" Multiplication | |
229 mulwf isr_xB | |
230 movf PRODL, w ; Add the Lower Byte Next | |
231 addwf isr_xC+1, f | |
232 movf PRODH, w ; Add the High Byte First | |
233 addwfc isr_xC+2, f | |
234 btfsc STATUS, C ; Add the Carry | |
235 incf isr_xC+3, f | |
236 movf isr_xA+1, w ; Do the "F" Multiplication | |
237 mulwf isr_xB+1 | |
238 movf PRODL, w | |
239 addwf isr_xC+2, f | |
240 movf PRODH, w | |
241 addwfc isr_xC+3, f | |
242 return | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
243 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
244 ;============================================================================= |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
245 ; 24bit shift, repeted WREG times. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
246 ; Because we shift less than 8bits, and keep only C[2:1], we don't care what |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
247 ; bit is inserted... |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
248 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
249 isr_shift_C31: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
250 rrcf isr_xC+3,F ; Shift the three bytes... |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
251 rrcf isr_xC+2,F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
252 rrcf isr_xC+1,F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
253 decfsz WREG |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
254 bra isr_shift_C31 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
255 return |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
256 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
257 ;============================================================================= |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
258 ; s16 * s16 --> 32bit multiply (xA * xB --> xC) |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
259 ; Signed multiplication. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
260 ; Code from... the Pic18F documentation ;-) |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
261 isr_unsigned_mult16x16: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
262 MOVF isr_xA+0, W ; Lowest is simply a[0] * b[0] |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
263 MULWF isr_xB+0 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
264 MOVFF PRODL, isr_xC+0 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
265 MOVFF PRODH, isr_xC+1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
266 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
267 MOVF isr_xA+1, W ; And highest a[1] * b[1] |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
268 MULWF isr_xB+1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
269 MOVFF PRODL, isr_xC+2 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
270 MOVFF PRODH, isr_xC+3 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
271 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
272 MOVF isr_xA+0, W ; Intermediates do propagate: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
273 MULWF isr_xB+1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
274 MOVF PRODL, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
275 ADDWF isr_xC+1, F ; Add cross products |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
276 MOVF PRODH, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
277 ADDWFC isr_xC+2, F ; with propagated carry |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
278 CLRF WREG |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
279 ADDWFC isr_xC+3, F ; on the three bytes. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
280 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
281 MOVF isr_xA+1, W ; And the second one, similarly. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
282 MULWF isr_xB+0 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
283 MOVF PRODL, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
284 ADDWF isr_xC+1, F ; Add cross products |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
285 MOVF PRODH, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
286 ADDWFC isr_xC+2, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
287 CLRF WREG |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
288 ADDWFC isr_xC+3, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
289 return |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
290 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
291 isr_signed_mult16x16: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
292 rcall isr_unsigned_mult16x16 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
293 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
294 ; Manage sign extension of operand B |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
295 BTFSS isr_xB+1,7 ; Is B negatif ? |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
296 BRA isr_signed_mult_checkA ; No: check ARG1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
297 MOVF isr_xA+0, W ; Yes: add -65536 * A |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
298 SUBWF isr_xC+2, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
299 MOVF isr_xA+1, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
300 SUBWFB isr_xC+3, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
301 ; And of operand A |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
302 isr_signed_mult_checkA |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
303 BTFSS isr_xA+1, 7 ; Is A negatif ? |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
304 RETURN ; No: done |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
305 MOVF isr_xB+0, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
306 SUBWF isr_xC+2, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
307 MOVF isr_xB+1, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
308 SUBWFB isr_xC+3, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
309 RETURN |