Mercurial > public > mk2
annotate code_part1/OSTC_code_asm_part1/math.asm @ 186:57a654c199ae
BUGFIX Gradient Factor algo.
+ Fix calc_hauptroutine_update_tissues() crazzy calculation in GF mode for deco limit.
+ Backup lock/GF_step/deco_pointer during ascent simulation.
author | JeanDo |
---|---|
date | Wed, 02 Feb 2011 00:09:30 +0100 |
parents | 8d6aca08f66b |
children | 06299199dfb9 |
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 | |
56 btfsc STATUS,C | |
0 | 57 return ; result positve |
58 ; sub_c = sub_a - sub_b | |
59 bsf neg_flag ; result negative | |
60 movff sub_c+0,sub_b+0 | |
61 movff sub_c+1,sub_b+1 | |
62 setf sub_a | |
63 setf sub_a+1 | |
74 | 64 movf sub_b+0, W ; Get Value to be subtracted |
65 subwf sub_a+0, W ; Do the High Byte | |
66 movwf sub_c+0 | |
67 movf sub_b+1, W ; Get the Value to be Subbed | |
68 subwfb sub_a+1, W | |
0 | 69 movwf sub_c+1 |
70 return | |
71 | |
72 | |
105 | 73 mult16x16: ;xA*xB=xC |
0 | 74 clrf xC+2 ; Clear the High-Order Bits |
75 clrf xC+3 | |
76 movf xA, w ; Do the "L" Multiplication first | |
77 mulwf xB | |
78 movf PRODL, w ; Save result | |
79 movwf xC | |
80 movf PRODH, w | |
81 movwf xC+1 | |
82 movf xA, w ; Do the "I" Multiplication | |
83 mulwf xB+1 | |
84 movf PRODL, w ; Save the Most Significant Byte First | |
85 addwf xC+1, f | |
86 movf PRODH, w | |
87 addwfc xC+2, f ; Add to the Last Result | |
88 movf xA+1, w ; Do the "O" Multiplication | |
89 mulwf xB | |
90 movf PRODL, w ; Add the Lower Byte Next | |
91 addwf xC+1, f | |
92 movf PRODH, w ; Add the High Byte First | |
93 addwfc xC+2, f | |
94 btfsc STATUS, C ; Add the Carry | |
95 incf xC+3, f | |
96 movf xA+1, w ; Do the "F" Multiplication | |
97 mulwf xB+1 | |
98 movf PRODL, w | |
99 addwf xC+2, f | |
100 movf PRODH, w | |
101 addwfc xC+3, f | |
102 return | |
103 | |
104 | |
105 div16x16: ;xA/xB=xC with xA as remainder | |
106 ;uses divB as temp variable | |
107 clrf xC+0 | |
108 clrf xC+1 | |
109 MOVF xB+0,W ; Check for zero | |
110 IORWF xB+1,W ; | |
111 BTFSC STATUS,Z ; Check for zero | |
112 RETLW H'FF' ; return 0xFF if illegal | |
113 MOVLW 1 ; Start count at 1 | |
114 MOVWF divB ; Clear Count | |
115 div16x16_1 | |
116 BTFSC xB+1,7 ; High bit set ? | |
117 bra div16x16_2 ; Yes then continue | |
118 INCF divB,F ; Increment count | |
119 | |
120 bcf STATUS,C | |
121 rlcf xB+0,F | |
122 rlcf xB+1,F | |
123 bra div16x16_1 | |
124 div16x16_2: | |
125 ; Shift result left | |
126 bcf STATUS,C | |
127 rlcf xC+0,F | |
128 rlcf xC+1,F | |
129 | |
130 ; Reduce Divisor | |
131 | |
132 MOVF xB,W ; Get low byte of subtrahend | |
133 SUBWF xA,F ; Subtract DST(low) - SRC(low) | |
134 MOVF xB+1,W ; Now get high byte of subtrahend | |
135 BTFSS STATUS,C ; If there was a borrow, rather than | |
136 INCF xB+1,W ; decrement high byte of dst we inc src | |
137 SUBWF xA+1,F ; Subtract the high byte and we're done | |
138 | |
139 | |
140 BTFSC STATUS, C ; Did it reduce? | |
141 bra div16x16_3 ; No, so it was less than | |
142 | |
143 movf xB+0,W ; Reverse subtraction | |
144 addwf xA+0,F | |
145 movf xB+1,W | |
146 addwfc xA+1,F | |
147 | |
148 bra div16x16_4 ; Continue the process | |
149 div16x16_3: | |
150 BSF xC+0,0 ; Yes it did, this gets a 1 bit | |
151 div16x16_4: | |
152 DECF divB,F ; Decrement N_COUNT | |
153 BTFSC STATUS,Z ; If its not zero then continue | |
154 return | |
155 | |
156 bcf STATUS,C | |
157 rrcf xB+1,F | |
158 rrcf xB+0,F | |
159 | |
160 bra div16x16_2 ; Next bit. | |
161 | |
162 div32x16: ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder | |
163 ; Setup | |
164 movlw .32 ; setup shift counter | |
165 movwf divB | |
166 movf xC+3,W ; move ACCb to ACCf | |
167 movwf xA+1 | |
168 movf xC+2,W | |
169 movwf xA+0 | |
170 movf xC+1,W ; move ACCc to ACCe | |
171 movwf sub_a+1 | |
172 movf xC+0,W | |
173 movwf sub_a+0 | |
174 clrf xC+3 | |
175 clrf xC+2 | |
176 clrf xC+1 | |
177 clrf xC+0 | |
178 clrf sub_b+1 | |
179 clrf sub_b+0 | |
180 div32x16_2 | |
181 bcf STATUS,C | |
182 rlcf sub_a+0,F | |
183 rlcf sub_a+1,F | |
184 rlcf xA+0,F | |
185 rlcf xA+1,F | |
186 rlcf sub_b+0,F | |
187 rlcf sub_b+1,F | |
188 movf xB+1,W | |
189 subwf sub_b+1,W ; check if a>d | |
190 btfss STATUS,Z | |
191 goto div32x16_3 | |
192 movf xB+0,W | |
193 subwf sub_b+0,W ; if msb equal then check lsb | |
194 div32x16_3 | |
195 btfss STATUS,C ; carry set if d>a | |
196 goto div32x16_4 | |
197 movf xB+0,W ; d-a into d | |
198 subwf sub_b+0,F | |
199 btfss STATUS,C | |
200 decf sub_b+1,F | |
201 movf xB+1,W | |
202 subwf sub_b+1,F | |
203 bsf STATUS,C ; shift a 1 into b (result) | |
204 div32x16_4 | |
205 rlcf xC+0,F | |
206 rlcf xC+1,F | |
207 rlcf xC+2,F | |
208 rlcf xC+3,F | |
209 decfsz divB,F ; loop until all bits checked | |
210 goto div32x16_2 | |
211 return | |
212 | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
213 ;============================================================================= |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
214 ; u16 * u16 --> 32bit multiply (xA * xB --> xC) |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
215 ; Used in interupt service routines, to compute temperature and pressure. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
216 ; |
0 | 217 isr_mult16x16: |
218 clrf isr_xC+2 ; Clear the High-Order Bits | |
219 clrf isr_xC+3 | |
220 movf isr_xA, w ; Do the "L" Multiplication first | |
221 mulwf isr_xB | |
222 movf PRODL, w ; Save result | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
223 movwf isr_xC+0 |
0 | 224 movf PRODH, w |
225 movwf isr_xC+1 | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
226 movf isr_xA+0, w ; Do the "I" Multiplication |
0 | 227 mulwf isr_xB+1 |
228 movf PRODL, w ; Save the Most Significant Byte First | |
229 addwf isr_xC+1, f | |
230 movf PRODH, w | |
231 addwfc isr_xC+2, f ; Add to the Last Result | |
232 movf isr_xA+1, w ; Do the "O" Multiplication | |
233 mulwf isr_xB | |
234 movf PRODL, w ; Add the Lower Byte Next | |
235 addwf isr_xC+1, f | |
236 movf PRODH, w ; Add the High Byte First | |
237 addwfc isr_xC+2, f | |
238 btfsc STATUS, C ; Add the Carry | |
239 incf isr_xC+3, f | |
240 movf isr_xA+1, w ; Do the "F" Multiplication | |
241 mulwf isr_xB+1 | |
242 movf PRODL, w | |
243 addwf isr_xC+2, f | |
244 movf PRODH, w | |
245 addwfc isr_xC+3, f | |
246 return | |
161
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
247 |
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 ; 24bit shift, repeted WREG times. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
250 ; 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
|
251 ; bit is inserted... |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
252 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
253 isr_shift_C31: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
254 rrcf isr_xC+3,F ; Shift the three bytes... |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
255 rrcf isr_xC+2,F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
256 rrcf isr_xC+1,F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
257 decfsz WREG |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
258 bra isr_shift_C31 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
259 return |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
260 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
261 ;============================================================================= |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
262 ; s16 * s16 --> 32bit multiply (xA * xB --> xC) |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
263 ; Signed multiplication. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
264 ; Code from... the Pic18F documentation ;-) |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
265 isr_unsigned_mult16x16: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
266 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
|
267 MULWF isr_xB+0 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
268 MOVFF PRODL, isr_xC+0 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
269 MOVFF PRODH, isr_xC+1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
270 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
271 MOVF isr_xA+1, W ; And highest a[1] * b[1] |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
272 MULWF isr_xB+1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
273 MOVFF PRODL, isr_xC+2 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
274 MOVFF PRODH, isr_xC+3 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
275 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
276 MOVF isr_xA+0, W ; Intermediates do propagate: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
277 MULWF isr_xB+1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
278 MOVF PRODL, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
279 ADDWF isr_xC+1, F ; Add cross products |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
280 MOVF PRODH, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
281 ADDWFC isr_xC+2, F ; with propagated carry |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
282 CLRF WREG |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
283 ADDWFC isr_xC+3, F ; on the three bytes. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
284 ; |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
285 MOVF isr_xA+1, W ; And the second one, similarly. |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
286 MULWF isr_xB+0 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
287 MOVF PRODL, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
288 ADDWF isr_xC+1, F ; Add cross products |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
289 MOVF PRODH, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
290 ADDWFC isr_xC+2, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
291 CLRF WREG |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
292 ADDWFC isr_xC+3, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
293 return |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
294 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
295 isr_signed_mult16x16: |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
296 rcall isr_unsigned_mult16x16 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
297 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
298 ; Manage sign extension of operand B |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
299 BTFSS isr_xB+1,7 ; Is B negatif ? |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
300 BRA isr_signed_mult_checkA ; No: check ARG1 |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
301 MOVF isr_xA+0, W ; Yes: add -65536 * A |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
302 SUBWF isr_xC+2, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
303 MOVF isr_xA+1, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
304 SUBWFB isr_xC+3, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
305 ; And of operand A |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
306 isr_signed_mult_checkA |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
307 BTFSS isr_xA+1, 7 ; Is A negatif ? |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
308 RETURN ; No: done |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
309 MOVF isr_xB+0, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
310 SUBWF isr_xC+2, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
311 MOVF isr_xB+1, W |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
312 SUBWFB isr_xC+3, F |
8d6aca08f66b
Use signed arithmetic for pressure/temperature compensation.
JeanDo
parents:
105
diff
changeset
|
313 RETURN |