Mercurial > public > mk2
annotate code_part1/OSTC_code_asm_part1/altimeter.asm @ 200:0a3ca358c684
BUGFIX clear decotable at begin of simulator mode.
+ minor P2_deco cleanups and comments.
author | JeanDo |
---|---|
date | Mon, 14 Feb 2011 23:15:40 +0100 |
parents | f5e9db793dd3 |
children | d19445e0cb2f |
rev | line source |
---|---|
125 | 1 ;============================================================================= |
2 ; | |
3 ; File altimeter.asm | |
4 ; | |
5 ; Altimeter function prototype. | |
6 ; | |
7 ; This program is free software: you can redistribute it and/or modify | |
8 ; it under the terms of the GNU General Public License as published by | |
9 ; the Free Software Foundation, either version 3 of the License, or | |
10 ; (at your option) any later version. | |
11 ; | |
12 ; This program is distributed in the hope that it will be useful, | |
13 ; but WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
15 ; GNU General Public License for more details. | |
16 ; | |
17 ; You should have received a copy of the GNU General Public License | |
18 ; along with this program. If not, see <http://www.gnu.org/licenses/>. | |
19 ; | |
20 ; Copyright (c) 2010, JD Gascuel. | |
21 ;============================================================================= | |
22 ; HISTORY | |
23 ; 2010-12-15 : [jDG] First prototype with quadratic polynomial ant tp°. | |
24 ; 2010-12-28 : [jDG] Use MPLAB Math and C libraries for FP32 computations. | |
25 ; 2011-01-02 : [jDG] Edit reference pressure by 0.25 mbar. | |
26 ; | |
132
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
27 ; Known bug: Simulator reset altitude and reference... |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
28 |
125 | 29 altimeter_calc: |
30 movlb HIGH(pressureAvg) | |
31 | |
170 | 32 movlw HIGH(4*.900) ; Is presure ref bigger than 900mbar |
33 cpfsgt pressureRef+1 | |
34 bra altimeter_reset ; No: Should do a reset now. | |
162 | 35 |
36 movlw HIGH(4*.1100) ; Is ref pressure bigger than 1100mbar ? | |
37 cpfsgt pressureRef+1 | |
38 bra altimeter_1 ; No: ok it is valid... | |
39 | |
170 | 40 ; Reset computation. Eg. after a sleep, enables to faster restart with correct |
41 ; values... | |
42 altimeter_reset: | |
43 movlb HIGH(pressureAvg) | |
125 | 44 movlw LOW(4*.1013+1) ; Init see level at 1013,25 mbar. |
45 movwf pressureRef+0 | |
46 movlw HIGH(4*.1013+1) | |
47 movwf pressureRef+1 | |
48 | |
49 clrf pressureSum+0 ; Init averaging area | |
50 clrf pressureSum+1 | |
51 clrf pressureCount | |
52 | |
53 movff amb_pressure+0,pressureAvg+0 ; And init first average. | |
54 movff amb_pressure+1,pressureAvg+1 | |
55 | |
56 movlw 4 ; And multiply AVG by 16 to be coherent. | |
57 altimeter_reset_1: | |
58 bcf STATUS,C | |
59 rlcf pressureAvg+0 | |
60 rlcf pressureAvg+1 | |
61 decfsz WREG | |
62 bra altimeter_reset_1 | |
132
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
63 |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
64 rcall compute_altitude |
125 | 65 |
66 movlb 1 ; Back to normal bank1. | |
67 return | |
68 | |
69 altimeter_1: | |
70 ;---- Do a bank-safe 16bit summing ----------------------------------- | |
71 movff amb_pressure+0,WREG | |
72 addwf pressureSum+0,F | |
73 movff amb_pressure+1,WREG | |
74 addwfc pressureSum+1,F | |
75 | |
76 incf pressureCount ; Increment count too. | |
77 movlw .32 ; Already 32 done ? | |
78 subwf pressureCount,W | |
79 bnz altimeter_4 ; NO: skip update. | |
80 | |
81 ;---- Update altitude every 32 pressure measures -------------------- | |
82 bcf STATUS,C ; Divide by 2, to store pressure x16 | |
83 rrcf pressureSum+1 | |
84 rrcf pressureSum+0 | |
85 | |
86 movff pressureSum+0,pressureAvg+0 | |
87 movff pressureSum+1,pressureAvg+1 | |
88 | |
89 rcall compute_altitude ; Compute from the averaged value... | |
90 | |
91 clrf pressureSum+0 ; And reset sum zone for next averaging. | |
92 clrf pressureSum+1 | |
93 clrf pressureCount | |
94 | |
95 altimeter_4: | |
96 movlb 1 ; make sure to be in normal bank1 | |
97 return | |
98 | |
99 ;---- Display result ------------------------------------------------- | |
100 altimeter_display: | |
101 GETCUSTOM8 .49 ; Check CF#49 | |
102 btfss WREG,0 ; Enabled ? | |
103 return ; NO: return | |
104 | |
105 WIN_TOP .35 ; Custom view drawing zone... | |
106 WIN_LEFT .90 | |
107 WIN_INVERT .0 | |
108 WIN_FONT .0 | |
109 call PLED_standard_color | |
110 | |
166 | 111 STRCPY "Alt: " |
125 | 112 |
113 movff altitude+0,lo ; BANK-SAFE read altitude | |
114 movff altitude+1,hi | |
166 | 115 btfss hi,7 ; Is altitude negativ ? |
116 bra altimeter_2 ; No: just print it | |
125 | 117 |
166 | 118 PUTC '-' ; Yes: print the minus sign |
119 comf hi ; And do a 16bit 2-complement. | |
120 comf lo | |
121 infsnz lo | |
122 incf hi | |
123 | |
124 altimeter_2: | |
125 | 125 bsf leftbind |
126 output_16 | |
127 bcf leftbind | |
128 | |
166 | 129 STRCAT_PRINT "m " |
125 | 130 return |
131 | |
132 ;============================================================================= | |
133 ; Compute altitude, using the formula: | |
134 ; H(P) = 18.787 log10(P0/P) (Log base 10) | |
135 | |
136 ;---- Interface the the Mayh library ----------------------------------------- | |
137 extern __AARGB2 ; A float in fA2, fA1, fA0, fAExo | |
138 extern __BARGB2 ; B float in fB2, fB1, fB0, fBExo | |
139 extern FLO1632U ; convert uint16 fA+1 --> fp32 fA | |
140 extern FPD32 ; fp32 divide fA/fB --> fA | |
141 extern FPM32 ; fp32 multiply fA*fB --> fA | |
142 extern INT3216 ; convert fp32 fA --> int16 fA+1 | |
143 #define fA __AARGB2 | |
144 #define fB __BARGB2 | |
145 | |
146 ;---- Interface to the C library --------------------------------------------- | |
147 extern __AARGB3 | |
148 extern log10 ; float32 log(auto float32) | |
149 #define fRet __AARGB3 | |
150 | |
151 compute_altitude: | |
152 ; Setup C-code stack, to enable calling the log() function. | |
153 lfsr FSR1, c_code_data_stack | |
154 lfsr FSR2, c_code_data_stack | |
155 | |
156 ; Convert pressure to float, --> fB | |
157 movff pressureAvg+0, fA+1 | |
158 movff pressureAvg+1, fA+2 | |
159 call FLO1632U ; u16 fA[1:2] --> fp32 fA | |
160 movff fA+0, fB+0 | |
161 movff fA+1, fB+1 | |
162 movff fA+2, fB+2 | |
163 movff fA+3, fB+3 | |
164 | |
165 ; Convert sea-level reference pressure to float, --> fB | |
166 movff pressureRef+0, fA+1 ; Get sea level pressure. | |
167 movff pressureRef+1, fA+2 | |
168 bcf STATUS,C ; Multiply by 4. | |
169 rlcf fA+1 | |
170 rlcf fA+2 | |
171 bcf STATUS,C | |
172 rlcf fA+1 | |
173 rlcf fA+2 | |
174 call FLO1632U ; to float: u16 fA[1:2] --> fp32 fA | |
175 | |
176 ; Divide | |
177 call FPD32 ; fp32 X/Y --> X | |
178 | |
179 ; log10() | |
180 movff fA+0, POSTINC1 ; Push X to stack | |
181 movff fA+1, POSTINC1 | |
182 movff fA+2, POSTINC1 | |
183 movff fA+3, POSTINC1 | |
184 call log10 ; log(P0/P) | |
185 | |
186 movf POSTDEC1,F,ACCESS ; pop argument | |
187 movf POSTDEC1,F,ACCESS | |
188 movf POSTDEC1,F,ACCESS | |
189 movf POSTDEC1,F,ACCESS | |
190 | |
191 ; Move log10(P0/P) to fB | |
192 movff fRet+0,fB+0 ; move result to fB | |
193 movff fRet+1,fB+1 | |
194 movff fRet+2,fB+2 | |
195 movff fRet+3,fB+3 | |
196 | |
197 ; Multiply by scaling factor for meters, and standatd atmosphere. | |
198 movlw LOW(.18787) | |
199 movff WREG, fA+1 | |
200 movlw HIGH(.18787) | |
201 movff WREG, fA+2 | |
202 call FLO1632U ; u16 fA[1:2] --> fp32 fA | |
203 call FPM32 ; altitute --> fp32 fA | |
204 | |
205 ; Convert result to int16 --> altitude. | |
206 call INT3216 ; fp32 fA --> int16 fA+1 | |
207 movff fA+1, altitude+0 | |
208 movff fA+2, altitude+1 | |
209 | |
210 return | |
211 | |
212 ;============================================================================= | |
213 ; Altimeter menu | |
214 ; | |
215 ; Edit reference (where altitude = 0) pressure, while displaying corresponding | |
216 ; altitude. | |
217 ; | |
218 altimeter_menu: | |
132
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
219 movff pressureRef+0,WREG ; Make sure it is initialized... |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
220 movff pressureRef+1,fA |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
221 iorwf fA |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
222 bnz altimeter_menu_1 ; Yes: skip reset... |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
223 rcall altimeter_reset |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
224 |
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
225 altimeter_menu_1: |
125 | 226 call PLED_ClearScreen ; Menu header. |
227 call PLED_standard_color | |
228 call PLED_topline_box | |
229 WIN_INVERT .1 ; Init new Wordprocessor | |
174 | 230 DISPLAYTEXTH .288 ; Title bar |
125 | 231 |
170 | 232 movlw 2 ; Start menu on line 2. |
125 | 233 movwf menupos |
234 | |
235 altimeter_menu_2: | |
174 | 236 WIN_INVERT 0 |
237 WIN_FONT 0 | |
125 | 238 WIN_LEFT .20 ; First line: |
239 WIN_TOP .35 | |
174 | 240 lfsr FSR2,letter |
241 OUTPUTTEXTH .289 ; Sea ref: | |
125 | 242 |
243 movff pressureRef+0, lo | |
244 movff pressureRef+1, hi | |
245 bcf STATUS,C ; Divide ref pressure by 4 | |
246 rrcf hi ; to get the integer part of it: | |
247 rrcf lo | |
248 bcf STATUS,C | |
249 rrcf hi | |
250 rrcf lo | |
251 bsf leftbind | |
252 output_16 | |
253 | |
170 | 254 STRCAT_PRINT " mbar " |
125 | 255 |
170 | 256 WIN_TOP .65 ; Action enable |
174 | 257 lfsr FSR2, letter |
258 OUTPUTTEXTH .290 | |
125 | 259 GETCUSTOM8 .49 |
260 btfss WREG,0 | |
261 bra alt_menu_1 | |
199 | 262 OUTPUTTEXT .130 ; ON |
125 | 263 bra alt_menu_2 |
264 alt_menu_1: | |
199 | 265 OUTPUTTEXT .131 ; OFF |
125 | 266 alt_menu_2: |
199 | 267 call word_processor |
125 | 268 |
174 | 269 DISPLAYTEXTH .291 ; Action reset |
270 DISPLAYTEXTH .292 ; Action add | |
271 DISPLAYTEXTH .293 ; Action sub | |
272 DISPLAYTEXT .011 ; Action exit | |
125 | 273 |
174 | 274 WIN_LEFT .85 ; Bottom right. |
275 lfsr FSR2, letter | |
276 OUTPUTTEXTH .294 ; "Alt: " | |
277 | |
170 | 278 movff altitude+0, lo |
279 movff altitude+1, hi | |
280 btfss hi,7 ; Is altitude negativ ? | |
281 bra altimeter_menu_3 ; No: just print it | |
282 | |
283 PUTC '-' ; Yes: print the minus sign | |
284 comf hi ; And do a 16bit 2-complement. | |
285 comf lo | |
286 infsnz lo | |
287 incf hi | |
288 | |
289 altimeter_menu_3: | |
290 bsf leftbind | |
291 output_16 | |
292 bcf leftbind | |
293 STRCAT_PRINT "m " | |
294 | |
125 | 295 alt_menu_loop: |
296 call PLED_menu_cursor ; Display cursor | |
297 bcf switch_left ; reset buttons state | |
298 bcf switch_right | |
299 | |
300 alt_menu_loop1: ; Wait for button. | |
301 btfsc switch_right ; [[MENU]] button | |
302 bra alt_menu_next | |
303 | |
304 btfsc switch_left ;[[ENTER]] button | |
305 bra alt_menu_do_it | |
306 | |
307 btfsc divemode ; Diving stared ? | |
308 goto restart ; YES: quit this menu ! | |
309 | |
310 btfsc onesecupdate ; Check what should be every 1sec. | |
311 call timeout_surfmode | |
312 | |
313 btfsc onesecupdate | |
314 call set_dive_modes | |
315 | |
316 bcf onesecupdate ; end of 1sek. tasks | |
317 | |
318 btfsc sleepmode ; Sleep mode entered ? | |
319 bra alt_menu_exit | |
320 | |
321 bra alt_menu_loop1 | |
322 | |
323 ;---- Move to next line ------------------------------------------------------ | |
324 | |
325 alt_menu_next: | |
326 incf menupos ; next line. | |
327 movlw .7 | |
328 cpfseq menupos ; Below last line ? | |
329 bra alt_menu_loop | |
170 | 330 movlw .2 ; Yes: back to line no 2. |
125 | 331 movwf menupos |
332 bra alt_menu_loop | |
333 | |
334 ;----- Execute menu line ----------------------------------------------------- | |
335 | |
336 alt_menu_do_it: | |
337 movf menupos,W ; test line value | |
170 | 338 addlw -2 |
339 bz alt_menu_enable ; 2 --> reset | |
340 dcfsnz WREG | |
341 bra alt_menu_reset ; 3 --> +1 | |
125 | 342 dcfsnz WREG |
343 bra alt_menu_plus1 ; 4 --> +1 | |
344 dcfsnz WREG | |
345 bra alt_menu_minus1 ; 5 --> -1 | |
346 bra alt_menu_exit ; else --> exit | |
347 | |
348 ;---- Toggle altimeter (CF#49) ----------------------------------------------- | |
349 alt_menu_enable: | |
350 GETCUSTOM8 .49 ; Read CF#49 | |
351 btg WREG,0 ; Toggle boolean value | |
352 movwf EEDATA | |
353 movlw d'1' ; Upper EEPROM Bank | |
354 movwf EEADRH | |
355 movlw 4*(.49-.32) + 0x82 ; CF#49 low byte address in EEPROM | |
356 movwf EEADR | |
357 call write_eeprom | |
358 bra altimeter_menu_2 | |
359 | |
170 | 360 ;---- Reset sea level pressure to reference ---------------------------------- |
361 alt_menu_reset: | |
362 rcall altimeter_reset | |
363 movlb 1 ; Go back to normal bank1 | |
364 bra altimeter_menu_2 | |
365 | |
125 | 366 ;---- Increment sea level pressure ------------------------------------------- |
367 alt_menu_plus1: | |
368 movlb HIGH(pressureRef) ; Setup our own ram bank | |
170 | 369 movlw 4 |
370 addwf pressureRef+0,F ; 16bit inc. | |
371 movlw 0 | |
372 addwfc pressureRef+1,F | |
125 | 373 bra alt_menu_recompute ; then recompute altitude. |
374 | |
375 ;---- Decrement sea level pressure ------------------------------------------- | |
376 alt_menu_minus1: | |
377 movlb HIGH(pressureRef) ; Setup our own ram bank | |
170 | 378 movlw -4 |
379 addwf pressureRef+0,F ; 16bit decrement | |
380 movlw -1 | |
381 addwfc pressureRef+1,F | |
125 | 382 |
383 alt_menu_recompute: | |
384 rcall compute_altitude ; Recompute altitude | |
385 movlb 1 ; Go back to normal bank1 | |
386 bra altimeter_menu_2 | |
387 | |
388 ;---- Exit altimeter menu ---------------------------------------------------- | |
389 alt_menu_exit: | |
390 movlw .5 ; reset position to Altimeter line. | |
391 movwf menupos ; | |
392 goto more_menu2 ; in the More... menu. |