Mercurial > public > mk2

annotate code_part1/OSTC_code_asm_part1/math.asm @ 261:9d4ab7c96885

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Fix first stop rounding in GF model.
author JeanDo
date Mon, 11 Apr 2011 09:00:49 +0200
parents 8d6aca08f66b
children 06299199dfb9
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
1 ; OSTC - diving computer code
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
2 ; Copyright (C) 2008 HeinrichsWeikamp GbR
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
3
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
4 ; This program is free software: you can redistribute it and/or modify
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
5 ; it under the terms of the GNU General Public License as published by
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
6 ; the Free Software Foundation, either version 3 of the License, or
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
7 ; (at your option) any later version.
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
8
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
9 ; This program is distributed in the hope that it will be useful,
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
10 ; but WITHOUT ANY WARRANTY; without even the implied warranty of
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
11 ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
12 ; GNU General Public License for more details.
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
13
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
14 ; You should have received a copy of the GNU General Public License
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
15 ; along with this program. If not, see <http://www.gnu.org/licenses/>.
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
16
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
17
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
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
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
23 ; known bugs:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
24 ; ToDo: clean up!
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
25
88
heinrichsweikamp
parents: 74
diff changeset
26
0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
27 convert_time: ; converts hi:lo in minutes to hours (hi) and minutes (lo)
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
28 movff lo,xA+0 ; divide by 60...
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
29 movff hi,xA+1 ;
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
30 movlw d'60' ;
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
31 movwf xB+0 ;
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
32 clrf xB+1 ;
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
33 rcall div16x16 ; xA/xB=xC with xA as remainder
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
34 movff xC+0,hi ; Hours
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
35 movff xA+0,lo ; =remaining minutes (0.....59)
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
36 return
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
37
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
38 div16:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
39 ; divA=divA/2^divB (divB: 8Bit only!)
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
40 bcf STATUS,C
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
41 rrcf divA+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
42 rrcf divA
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
43 decfsz divB
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
44 bra div16
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
45 return
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
46
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
47 sub16:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
48 ; sub_c = sub_a - sub_b
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
49 bcf neg_flag
74
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
50 movf sub_b+0, W ; Get Value to be subtracted
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
51 subwf sub_a+0, W ; Do the High Byte
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
52 movwf sub_c+0
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
53 movf sub_b+1, W ; Get the Value to be Subbed
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
54 subwfb sub_a+1, W
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
55 movwf sub_c+1
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
56 btfsc STATUS,C
0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
57 return ; result positve
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
58 ; sub_c = sub_a - sub_b
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
59 bsf neg_flag ; result negative
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
60 movff sub_c+0,sub_b+0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
61 movff sub_c+1,sub_b+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
62 setf sub_a
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
63 setf sub_a+1
74
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
64 movf sub_b+0, W ; Get Value to be subtracted
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
65 subwf sub_a+0, W ; Do the High Byte
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
66 movwf sub_c+0
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
67 movf sub_b+1, W ; Get the Value to be Subbed
a1ccb92b59cd 1.72beta release
heinrichsweikamp
parents: 0
diff changeset
68 subwfb sub_a+1, W
0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
69 movwf sub_c+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
70 return
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
71
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
72
105
64109f6fb3d1 added END to GasSetup, small fixes
heinrichsweikamp
parents: 88
diff changeset
73 mult16x16: ;xA*xB=xC
0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
74 clrf xC+2 ; Clear the High-Order Bits
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
75 clrf xC+3
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
76 movf xA, w ; Do the "L" Multiplication first
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
77 mulwf xB
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
78 movf PRODL, w ; Save result
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
79 movwf xC
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
80 movf PRODH, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
81 movwf xC+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
82 movf xA, w ; Do the "I" Multiplication
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
83 mulwf xB+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
84 movf PRODL, w ; Save the Most Significant Byte First
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
85 addwf xC+1, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
86 movf PRODH, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
87 addwfc xC+2, f ; Add to the Last Result
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
88 movf xA+1, w ; Do the "O" Multiplication
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
89 mulwf xB
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
90 movf PRODL, w ; Add the Lower Byte Next
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
91 addwf xC+1, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
92 movf PRODH, w ; Add the High Byte First
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
93 addwfc xC+2, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
94 btfsc STATUS, C ; Add the Carry
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
95 incf xC+3, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
96 movf xA+1, w ; Do the "F" Multiplication
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
97 mulwf xB+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
98 movf PRODL, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
99 addwf xC+2, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
100 movf PRODH, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
101 addwfc xC+3, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
102 return
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
103
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
104
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
105 div16x16: ;xA/xB=xC with xA as remainder
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
106 ;uses divB as temp variable
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
107 clrf xC+0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
108 clrf xC+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
109 MOVF xB+0,W ; Check for zero
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
110 IORWF xB+1,W ;
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
111 BTFSC STATUS,Z ; Check for zero
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
112 RETLW H'FF' ; return 0xFF if illegal
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
113 MOVLW 1 ; Start count at 1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
114 MOVWF divB ; Clear Count
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
115 div16x16_1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
116 BTFSC xB+1,7 ; High bit set ?
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
117 bra div16x16_2 ; Yes then continue
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
118 INCF divB,F ; Increment count
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
119
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
120 bcf STATUS,C
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
121 rlcf xB+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
122 rlcf xB+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
123 bra div16x16_1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
124 div16x16_2:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
125 ; Shift result left
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
126 bcf STATUS,C
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
127 rlcf xC+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
128 rlcf xC+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
129
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
130 ; Reduce Divisor
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
131
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
132 MOVF xB,W ; Get low byte of subtrahend
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
133 SUBWF xA,F ; Subtract DST(low) - SRC(low)
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
134 MOVF xB+1,W ; Now get high byte of subtrahend
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
135 BTFSS STATUS,C ; If there was a borrow, rather than
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
136 INCF xB+1,W ; decrement high byte of dst we inc src
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
137 SUBWF xA+1,F ; Subtract the high byte and we're done
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
138
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
139
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
140 BTFSC STATUS, C ; Did it reduce?
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
141 bra div16x16_3 ; No, so it was less than
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
142
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
143 movf xB+0,W ; Reverse subtraction
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
144 addwf xA+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
145 movf xB+1,W
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
146 addwfc xA+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
147
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
148 bra div16x16_4 ; Continue the process
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
149 div16x16_3:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
150 BSF xC+0,0 ; Yes it did, this gets a 1 bit
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
151 div16x16_4:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
152 DECF divB,F ; Decrement N_COUNT
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
153 BTFSC STATUS,Z ; If its not zero then continue
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
154 return
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
155
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
156 bcf STATUS,C
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
157 rrcf xB+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
158 rrcf xB+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
159
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
160 bra div16x16_2 ; Next bit.
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
161
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
162 div32x16: ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
163 ; Setup
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
164 movlw .32 ; setup shift counter
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
165 movwf divB
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
166 movf xC+3,W ; move ACCb to ACCf
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
167 movwf xA+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
168 movf xC+2,W
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
169 movwf xA+0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
170 movf xC+1,W ; move ACCc to ACCe
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
171 movwf sub_a+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
172 movf xC+0,W
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
173 movwf sub_a+0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
174 clrf xC+3
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
175 clrf xC+2
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
176 clrf xC+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
177 clrf xC+0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
178 clrf sub_b+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
179 clrf sub_b+0
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
180 div32x16_2
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
181 bcf STATUS,C
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
182 rlcf sub_a+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
183 rlcf sub_a+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
184 rlcf xA+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
185 rlcf xA+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
186 rlcf sub_b+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
187 rlcf sub_b+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
188 movf xB+1,W
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
189 subwf sub_b+1,W ; check if a>d
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
190 btfss STATUS,Z
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
191 goto div32x16_3
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
192 movf xB+0,W
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
193 subwf sub_b+0,W ; if msb equal then check lsb
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
194 div32x16_3
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
195 btfss STATUS,C ; carry set if d>a
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
196 goto div32x16_4
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
197 movf xB+0,W ; d-a into d
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
198 subwf sub_b+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
199 btfss STATUS,C
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
200 decf sub_b+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
201 movf xB+1,W
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
202 subwf sub_b+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
203 bsf STATUS,C ; shift a 1 into b (result)
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
204 div32x16_4
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
205 rlcf xC+0,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
206 rlcf xC+1,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
207 rlcf xC+2,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
208 rlcf xC+3,F
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
209 decfsz divB,F ; loop until all bits checked
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
210 goto div32x16_2
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
211 return
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
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
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
217 isr_mult16x16:
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
218 clrf isr_xC+2 ; Clear the High-Order Bits
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
219 clrf isr_xC+3
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
220 movf isr_xA, w ; Do the "L" Multiplication first
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
221 mulwf isr_xB
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
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
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
224 movf PRODH, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
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
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
227 mulwf isr_xB+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
228 movf PRODL, w ; Save the Most Significant Byte First
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
229 addwf isr_xC+1, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
230 movf PRODH, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
231 addwfc isr_xC+2, f ; Add to the Last Result
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
232 movf isr_xA+1, w ; Do the "O" Multiplication
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
233 mulwf isr_xB
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
234 movf PRODL, w ; Add the Lower Byte Next
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
235 addwf isr_xC+1, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
236 movf PRODH, w ; Add the High Byte First
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
237 addwfc isr_xC+2, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
238 btfsc STATUS, C ; Add the Carry
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
239 incf isr_xC+3, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
240 movf isr_xA+1, w ; Do the "F" Multiplication
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
241 mulwf isr_xB+1
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
242 movf PRODL, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
243 addwf isr_xC+2, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
244 movf PRODH, w
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
245 addwfc isr_xC+3, f
96a35aeda5f2 Initial setup
heinrichsweikamp
parents:
diff changeset
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