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