|
0
|
1 ;=============================================================================
|
|
|
2 ;
|
|
|
3 ; File math.asm
|
|
|
4 ;
|
|
|
5 ; Math subroutines
|
|
|
6 ;
|
|
|
7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved.
|
|
|
8 ;=============================================================================
|
|
|
9 ; HISTORY
|
|
|
10 ; 2011-08-03 : [mH] moving from OSTC code
|
|
|
11
|
|
|
12
|
|
275
|
13 #include "hwos.inc" ; Mandatory header
|
|
0
|
14
|
|
|
15 basic CODE
|
|
|
16 ;=============================================================================
|
|
|
17 global convert_time
|
|
|
18 convert_time: ; converts hi:lo in minutes to hours (hi) and minutes (lo)
|
|
|
19 movff lo,xA+0 ; divide by 60...
|
|
|
20 movff hi,xA+1 ;
|
|
|
21 movlw d'60' ;
|
|
|
22 movwf xB+0 ;
|
|
|
23 clrf xB+1 ;
|
|
|
24 rcall div16x16 ; xA/xB=xC with xA as remainder
|
|
|
25 movff xC+0,hi ; Hours
|
|
|
26 movff xA+0,lo ; =remaining minutes (0.....59)
|
|
|
27 return
|
|
|
28
|
|
|
29 global div16
|
|
|
30 div16:
|
|
|
31 ; divA=divA/2^divB (divB: 8Bit only!)
|
|
|
32 bcf STATUS,C
|
|
|
33 rrcf divA+1
|
|
|
34 rrcf divA+0
|
|
|
35 decfsz divB
|
|
|
36 bra div16
|
|
|
37 return
|
|
|
38
|
|
|
39 global sub16
|
|
|
40 sub16:
|
|
|
41 ; sub_c = sub_a - sub_b (with signed values)
|
|
|
42 bcf neg_flag
|
|
|
43 movf sub_b+0, W ; Get Value to be subtracted
|
|
|
44 subwf sub_a+0, W ; Do the High Byte
|
|
|
45 movwf sub_c+0
|
|
|
46 movf sub_b+1, W ; Get the Value to be Subbed
|
|
|
47 subwfb sub_a+1, W
|
|
|
48 movwf sub_c+1
|
|
|
49
|
|
|
50 btfss STATUS,N ; Negativ result ?
|
|
|
51 return ; NO: result positive done.
|
|
|
52
|
|
|
53 bsf neg_flag ; MARK result negative
|
|
|
54
|
|
|
55 comf sub_c+1 ; 16bit sign change.
|
|
|
56 negf sub_c+0
|
|
|
57 btfsc STATUS,C ; Carry to propagate ?
|
|
|
58 incf sub_c+1,F ; YES: do it.
|
|
|
59
|
|
|
60 return
|
|
|
61
|
|
|
62 global subU16
|
|
|
63 subU16:
|
|
|
64 ; sub_c = sub_a - sub_b (with UNSIGNED values)
|
|
|
65 bcf neg_flag
|
|
|
66 movf sub_b+0, W ; Get Value to be subtracted
|
|
|
67 subwf sub_a+0, W ; Do the High Byte
|
|
|
68 movwf sub_c+0
|
|
|
69 movf sub_b+1, W ; Get the Value to be Subbed
|
|
|
70 subwfb sub_a+1, W
|
|
|
71 movwf sub_c+1
|
|
|
72 btfsc STATUS,C ; Borrow to propagate ? (B == /CARRY)
|
|
|
73 return ; NO: result positive done.
|
|
|
74 bsf neg_flag ; MARK result negative
|
|
|
75 comf sub_c+1 ; 16bit sign change.
|
|
|
76 negf sub_c+0
|
|
|
77 btfsc STATUS,C ; Carry to propagate ?
|
|
|
78 incf sub_c+1,F ; YES: do it.
|
|
|
79 return
|
|
|
80
|
|
|
81 global mult16x16
|
|
|
82 mult16x16: ;xA*xB=xC
|
|
|
83 clrf xC+2 ; Clear the High-Order Bits
|
|
|
84 clrf xC+3
|
|
|
85 movf xA, w ; Do the "L" Multiplication first
|
|
|
86 mulwf xB
|
|
|
87 movf PRODL, w ; Save result
|
|
|
88 movwf xC
|
|
|
89 movf PRODH, w
|
|
|
90 movwf xC+1
|
|
|
91 movf xA, w ; Do the "I" Multiplication
|
|
|
92 mulwf xB+1
|
|
|
93 movf PRODL, w ; Save the Most Significant Byte First
|
|
|
94 addwf xC+1, f
|
|
|
95 movf PRODH, w
|
|
|
96 addwfc xC+2, f ; Add to the Last Result
|
|
|
97 movf xA+1, w ; Do the "O" Multiplication
|
|
|
98 mulwf xB
|
|
|
99 movf PRODL, w ; Add the Lower Byte Next
|
|
|
100 addwf xC+1, f
|
|
|
101 movf PRODH, w ; Add the High Byte First
|
|
|
102 addwfc xC+2, f
|
|
|
103 btfsc STATUS, C ; Add the Carry
|
|
|
104 incf xC+3, f
|
|
|
105 movf xA+1, w ; Do the "F" Multiplication
|
|
|
106 mulwf xB+1
|
|
|
107 movf PRODL, w
|
|
|
108 addwf xC+2, f
|
|
|
109 movf PRODH, w
|
|
|
110 addwfc xC+3, f
|
|
|
111 return
|
|
|
112
|
|
|
113 global div16x16
|
|
|
114 div16x16: ;xA/xB=xC with xA+0 as remainder
|
|
|
115 ;uses divB as temp variable
|
|
|
116 clrf xC+0
|
|
|
117 clrf xC+1
|
|
|
118 MOVF xB+0,W ; Check for zero
|
|
|
119 IORWF xB+1,W ;
|
|
|
120 BTFSC STATUS,Z ; Check for zero
|
|
|
121 RETLW H'FF' ; return 0xFF if illegal
|
|
|
122 MOVLW 1 ; Start count at 1
|
|
|
123 MOVWF divB ; Clear Count
|
|
|
124 div16x16_1:
|
|
|
125 BTFSC xB+1,7 ; High bit set ?
|
|
|
126 bra div16x16_2 ; Yes then continue
|
|
|
127 INCF divB,F ; Increment count
|
|
|
128
|
|
|
129 bcf STATUS,C
|
|
|
130 rlcf xB+0,F
|
|
|
131 rlcf xB+1,F
|
|
|
132 bra div16x16_1
|
|
|
133 div16x16_2:
|
|
|
134 ; Shift result left
|
|
|
135 bcf STATUS,C
|
|
|
136 rlcf xC+0,F
|
|
|
137 rlcf xC+1,F
|
|
|
138
|
|
|
139 ; Reduce Divisor
|
|
|
140
|
|
|
141 MOVF xB,W ; Get low byte of subtrahend
|
|
|
142 SUBWF xA,F ; Subtract DST(low) - SRC(low)
|
|
|
143 MOVF xB+1,W ; Now get high byte of subtrahend
|
|
|
144 BTFSS STATUS,C ; If there was a borrow, rather than
|
|
|
145 INCF xB+1,W ; decrement high byte of dst we inc src
|
|
|
146 SUBWF xA+1,F ; Subtract the high byte and we're done
|
|
|
147
|
|
|
148
|
|
|
149 BTFSC STATUS, C ; Did it reduce?
|
|
|
150 bra div16x16_3 ; No, so it was less than
|
|
|
151
|
|
|
152 movf xB+0,W ; Reverse subtraction
|
|
|
153 addwf xA+0,F
|
|
|
154 movf xB+1,W
|
|
|
155 addwfc xA+1,F
|
|
|
156
|
|
|
157 bra div16x16_4 ; Continue the process
|
|
|
158 div16x16_3:
|
|
|
159 BSF xC+0,0 ; Yes it did, this gets a 1 bit
|
|
|
160 div16x16_4:
|
|
|
161 DECF divB,F ; Decrement N_COUNT
|
|
|
162 BTFSC STATUS,Z ; If its not zero then continue
|
|
|
163 return
|
|
|
164
|
|
|
165 bcf STATUS,C
|
|
|
166 rrcf xB+1,F
|
|
|
167 rrcf xB+0,F
|
|
|
168
|
|
|
169 bra div16x16_2 ; Next bit.
|
|
|
170
|
|
|
171 global div32x16
|
|
|
172 div32x16: ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
|
|
|
173 ; Setup
|
|
|
174 movlw .32 ; setup shift counter
|
|
|
175 movwf divB
|
|
|
176 movff xC+3,xA+1 ; move ACCb to ACCf
|
|
|
177 movff xC+2,xA+0
|
|
|
178 movff xC+1,sub_a+1 ; move ACCc to ACCe
|
|
|
179 movff xC+0,sub_a+0
|
|
|
180 clrf xC+3
|
|
|
181 clrf xC+2
|
|
|
182 clrf xC+1
|
|
|
183 clrf xC+0
|
|
|
184 clrf sub_b+1
|
|
|
185 clrf sub_b+0
|
|
|
186 div32x16_2:
|
|
|
187 bcf STATUS,C
|
|
|
188 rlcf sub_a+0,F
|
|
|
189 rlcf sub_a+1,F
|
|
|
190 rlcf xA+0,F
|
|
|
191 rlcf xA+1,F
|
|
|
192 rlcf sub_b+0,F
|
|
|
193 rlcf sub_b+1,F
|
|
|
194 movf xB+1,W
|
|
|
195 subwf sub_b+1,W ; check if a>d
|
|
|
196 btfss STATUS,Z
|
|
|
197 bra div32x16_3
|
|
|
198 movf xB+0,W
|
|
|
199 subwf sub_b+0,W ; if msb equal then check lsb
|
|
|
200 div32x16_3:
|
|
|
201 btfss STATUS,C ; carry set if d>a
|
|
|
202 bra div32x16_4
|
|
|
203 movf xB+0,W ; d-a into d
|
|
|
204 subwf sub_b+0,F
|
|
|
205 btfss STATUS,C
|
|
|
206 decf sub_b+1,F
|
|
|
207 movf xB+1,W
|
|
|
208 subwf sub_b+1,F
|
|
|
209 bsf STATUS,C ; shift a 1 into b (result)
|
|
|
210 div32x16_4:
|
|
|
211 rlcf xC+0,F
|
|
|
212 rlcf xC+1,F
|
|
|
213 rlcf xC+2,F
|
|
|
214 rlcf xC+3,F
|
|
|
215 decfsz divB,F ; loop until all bits checked
|
|
|
216 bra div32x16_2
|
|
|
217 return
|
|
|
218
|
|
|
219 ;;=============================================================================
|
|
|
220 ;; u16 * u16 --> 32bit multiply (xA * xB --> xC)
|
|
|
221 ;; Used in interupt service routines, to compute temperature and pressure.
|
|
|
222 ;;
|
|
|
223 ; global isr_mult16x16
|
|
|
224 ;isr_mult16x16:
|
|
|
225 ; clrf isr_xC+2 ; Clear the High-Order Bits
|
|
|
226 ; clrf isr_xC+3
|
|
|
227 ; movf isr_xA, w ; Do the "L" Multiplication first
|
|
|
228 ; mulwf isr_xB
|
|
|
229 ; movf PRODL, w ; Save result
|
|
|
230 ; movwf isr_xC+0
|
|
|
231 ; movf PRODH, w
|
|
|
232 ; movwf isr_xC+1
|
|
|
233 ; movf isr_xA+0, w ; Do the "I" Multiplication
|
|
|
234 ; mulwf isr_xB+1
|
|
|
235 ; movf PRODL, w ; Save the Most Significant Byte First
|
|
|
236 ; addwf isr_xC+1, f
|
|
|
237 ; movf PRODH, w
|
|
|
238 ; addwfc isr_xC+2, f ; Add to the Last Result
|
|
|
239 ; movf isr_xA+1, w ; Do the "O" Multiplication
|
|
|
240 ; mulwf isr_xB
|
|
|
241 ; movf PRODL, w ; Add the Lower Byte Next
|
|
|
242 ; addwf isr_xC+1, f
|
|
|
243 ; movf PRODH, w ; Add the High Byte First
|
|
|
244 ; addwfc isr_xC+2, f
|
|
|
245 ; btfsc STATUS, C ; Add the Carry
|
|
|
246 ; incf isr_xC+3, f
|
|
|
247 ; movf isr_xA+1, w ; Do the "F" Multiplication
|
|
|
248 ; mulwf isr_xB+1
|
|
|
249 ; movf PRODL, w
|
|
|
250 ; addwf isr_xC+2, f
|
|
|
251 ; movf PRODH, w
|
|
|
252 ; addwfc isr_xC+3, f
|
|
|
253 ; return
|
|
|
254
|
|
|
255 ;=============================================================================
|
|
|
256 ; 24bit shift, repeted WREG times.
|
|
|
257 ; Because we shift less than 8bits, and keep only C[2:1], we don't care what
|
|
|
258 ; bit is inserted...
|
|
|
259 ;
|
|
|
260 global isr_shift_C31
|
|
|
261 isr_shift_C31:
|
|
|
262 rrcf isr_xC+3,F ; Shift the three bytes...
|
|
|
263 rrcf isr_xC+2,F
|
|
|
264 rrcf isr_xC+1,F
|
|
|
265 decfsz WREG
|
|
|
266 bra isr_shift_C31
|
|
|
267 return
|
|
|
268
|
|
|
269 ;=============================================================================
|
|
|
270 ; s16 * s16 --> 32bit multiply (xA * xB --> xC)
|
|
|
271 ; Signed multiplication.
|
|
|
272 ; Code from... the Pic18F documentation ;-)
|
|
|
273 global isr_unsigned_mult16x16
|
|
|
274 isr_unsigned_mult16x16:
|
|
|
275 MOVF isr_xA+0, W ; Lowest is simply a[0] * b[0]
|
|
|
276 MULWF isr_xB+0
|
|
|
277 MOVFF PRODL, isr_xC+0
|
|
|
278 MOVFF PRODH, isr_xC+1
|
|
|
279 ;
|
|
|
280 MOVF isr_xA+1, W ; And highest a[1] * b[1]
|
|
|
281 MULWF isr_xB+1
|
|
|
282 MOVFF PRODL, isr_xC+2
|
|
|
283 MOVFF PRODH, isr_xC+3
|
|
|
284 ;
|
|
|
285 MOVF isr_xA+0, W ; Intermediates do propagate:
|
|
|
286 MULWF isr_xB+1
|
|
|
287 MOVF PRODL, W
|
|
|
288 ADDWF isr_xC+1, F ; Add cross products
|
|
|
289 MOVF PRODH, W
|
|
|
290 ADDWFC isr_xC+2, F ; with propagated carry
|
|
|
291 CLRF WREG
|
|
|
292 ADDWFC isr_xC+3, F ; on the three bytes.
|
|
|
293 ;
|
|
|
294 MOVF isr_xA+1, W ; And the second one, similarly.
|
|
|
295 MULWF isr_xB+0
|
|
|
296 MOVF PRODL, W
|
|
|
297 ADDWF isr_xC+1, F ; Add cross products
|
|
|
298 MOVF PRODH, W
|
|
|
299 ADDWFC isr_xC+2, F
|
|
|
300 CLRF WREG
|
|
|
301 ADDWFC isr_xC+3, F
|
|
|
302 return
|
|
|
303
|
|
|
304 global isr_signed_mult16x16
|
|
|
305 isr_signed_mult16x16:
|
|
|
306 rcall isr_unsigned_mult16x16
|
|
|
307
|
|
|
308 ; Manage sign extension of operand B
|
|
|
309 BTFSS isr_xB+1,7 ; Is B negatif ?
|
|
|
310 BRA isr_signed_mult_checkA ; No: check ARG1
|
|
|
311 MOVF isr_xA+0, W ; Yes: add -65536 * A
|
|
|
312 SUBWF isr_xC+2, F
|
|
|
313 MOVF isr_xA+1, W
|
|
|
314 SUBWFB isr_xC+3, F
|
|
|
315 ; And of operand A
|
|
|
316 isr_signed_mult_checkA
|
|
|
317 BTFSS isr_xA+1, 7 ; Is A negatif ?
|
|
|
318 RETURN ; No: done
|
|
|
319 MOVF isr_xB+0, W
|
|
|
320 SUBWF isr_xC+2, F
|
|
|
321 MOVF isr_xB+1, W
|
|
|
322 SUBWFB isr_xC+3, F
|
|
|
323 RETURN
|
|
|
324
|
|
|
325 END |
