|
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 ;
|
|
|
27 altimeter_calc:
|
|
|
28 movlb HIGH(pressureAvg)
|
|
|
29
|
|
|
30 movf pressureRef+0,W ; Already initialized ?
|
|
|
31 iorwf pressureRef+1,W
|
|
|
32 bnz altimeter_1 ; Yes...
|
|
|
33
|
|
|
34 movlw LOW(4*.1013+1) ; Init see level at 1013,25 mbar.
|
|
|
35 movwf pressureRef+0
|
|
|
36 movlw HIGH(4*.1013+1)
|
|
|
37 movwf pressureRef+1
|
|
|
38
|
|
|
39 ; Reset computation. Eg. after a sleep, enables to faster restart with correct
|
|
|
40 ; values...
|
|
|
41 altimeter_reset:
|
|
|
42 movlb HIGH(pressureAvg)
|
|
|
43 clrf pressureSum+0 ; Init averaging area
|
|
|
44 clrf pressureSum+1
|
|
|
45 clrf pressureCount
|
|
|
46
|
|
|
47 clrf altitude+0 ; Mark as not computed yet.
|
|
|
48 clrf altitude+1
|
|
|
49
|
|
|
50 movff amb_pressure+0,pressureAvg+0 ; And init first average.
|
|
|
51 movff amb_pressure+1,pressureAvg+1
|
|
|
52
|
|
|
53 movlw 4 ; And multiply AVG by 16 to be coherent.
|
|
|
54 altimeter_reset_1:
|
|
|
55 bcf STATUS,C
|
|
|
56 rlcf pressureAvg+0
|
|
|
57 rlcf pressureAvg+1
|
|
|
58 decfsz WREG
|
|
|
59 bra altimeter_reset_1
|
|
|
60
|
|
|
61 movlb 1 ; Back to normal bank1.
|
|
|
62 return
|
|
|
63
|
|
|
64 altimeter_1:
|
|
|
65 ;---- Do a bank-safe 16bit summing -----------------------------------
|
|
|
66 movff amb_pressure+0,WREG
|
|
|
67 addwf pressureSum+0,F
|
|
|
68 movff amb_pressure+1,WREG
|
|
|
69 addwfc pressureSum+1,F
|
|
|
70
|
|
|
71 incf pressureCount ; Increment count too.
|
|
|
72 movlw .32 ; Already 32 done ?
|
|
|
73 subwf pressureCount,W
|
|
|
74 bnz altimeter_4 ; NO: skip update.
|
|
|
75
|
|
|
76 ;---- Update altitude every 32 pressure measures --------------------
|
|
|
77 bcf STATUS,C ; Divide by 2, to store pressure x16
|
|
|
78 rrcf pressureSum+1
|
|
|
79 rrcf pressureSum+0
|
|
|
80
|
|
|
81 movff pressureSum+0,pressureAvg+0
|
|
|
82 movff pressureSum+1,pressureAvg+1
|
|
|
83
|
|
|
84 rcall compute_altitude ; Compute from the averaged value...
|
|
|
85
|
|
|
86 clrf pressureSum+0 ; And reset sum zone for next averaging.
|
|
|
87 clrf pressureSum+1
|
|
|
88 clrf pressureCount
|
|
|
89
|
|
|
90 altimeter_4:
|
|
|
91 movlb 1 ; make sure to be in normal bank1
|
|
|
92 return
|
|
|
93
|
|
|
94 ;---- Display result -------------------------------------------------
|
|
|
95 altimeter_display:
|
|
|
96 GETCUSTOM8 .49 ; Check CF#49
|
|
|
97 btfss WREG,0 ; Enabled ?
|
|
|
98 return ; NO: return
|
|
|
99
|
|
|
100 WIN_TOP .35 ; Custom view drawing zone...
|
|
|
101 WIN_LEFT .90
|
|
|
102 WIN_INVERT .0
|
|
|
103 WIN_FONT .0
|
|
|
104 call PLED_standard_color
|
|
|
105
|
|
|
106 STRCPY "Alt:"
|
|
|
107
|
|
|
108 movff altitude+0,lo ; BANK-SAFE read altitude
|
|
|
109 movff altitude+1,hi
|
|
|
110 movf lo,W ; Is it zero (not computed yet) ?
|
|
|
111 iorwf hi,W
|
|
|
112 bz altimeter_2
|
|
|
113
|
|
|
114 bsf leftbind
|
|
|
115 output_16
|
|
|
116 bcf leftbind
|
|
|
117 bra altimeter_3
|
|
|
118
|
|
|
119 altimeter_2:
|
|
|
120 STRCAT "****"
|
|
|
121
|
|
|
122 altimeter_3:
|
|
|
123 STRCAT_PRINT "m "
|
|
|
124 return
|
|
|
125
|
|
|
126 ;=============================================================================
|
|
|
127 ; Compute altitude, using the formula:
|
|
|
128 ; H(P) = 18.787 log10(P0/P) (Log base 10)
|
|
|
129
|
|
|
130 ;---- Interface the the Mayh library -----------------------------------------
|
|
|
131 extern __AARGB2 ; A float in fA2, fA1, fA0, fAExo
|
|
|
132 extern __BARGB2 ; B float in fB2, fB1, fB0, fBExo
|
|
|
133 extern FLO1632U ; convert uint16 fA+1 --> fp32 fA
|
|
|
134 extern FPD32 ; fp32 divide fA/fB --> fA
|
|
|
135 extern FPM32 ; fp32 multiply fA*fB --> fA
|
|
|
136 extern INT3216 ; convert fp32 fA --> int16 fA+1
|
|
|
137 #define fA __AARGB2
|
|
|
138 #define fB __BARGB2
|
|
|
139
|
|
|
140 ;---- Interface to the C library ---------------------------------------------
|
|
|
141 extern __AARGB3
|
|
|
142 extern log10 ; float32 log(auto float32)
|
|
|
143 #define fRet __AARGB3
|
|
|
144
|
|
|
145 compute_altitude:
|
|
|
146 ; Setup C-code stack, to enable calling the log() function.
|
|
|
147 lfsr FSR1, c_code_data_stack
|
|
|
148 lfsr FSR2, c_code_data_stack
|
|
|
149
|
|
|
150 ; Convert pressure to float, --> fB
|
|
|
151 movff pressureAvg+0, fA+1
|
|
|
152 movff pressureAvg+1, fA+2
|
|
|
153 call FLO1632U ; u16 fA[1:2] --> fp32 fA
|
|
|
154 movff fA+0, fB+0
|
|
|
155 movff fA+1, fB+1
|
|
|
156 movff fA+2, fB+2
|
|
|
157 movff fA+3, fB+3
|
|
|
158
|
|
|
159 ; Convert sea-level reference pressure to float, --> fB
|
|
|
160 movff pressureRef+0, fA+1 ; Get sea level pressure.
|
|
|
161 movff pressureRef+1, fA+2
|
|
|
162 bcf STATUS,C ; Multiply by 4.
|
|
|
163 rlcf fA+1
|
|
|
164 rlcf fA+2
|
|
|
165 bcf STATUS,C
|
|
|
166 rlcf fA+1
|
|
|
167 rlcf fA+2
|
|
|
168 call FLO1632U ; to float: u16 fA[1:2] --> fp32 fA
|
|
|
169
|
|
|
170 ; Divide
|
|
|
171 call FPD32 ; fp32 X/Y --> X
|
|
|
172
|
|
|
173 ; log10()
|
|
|
174 movff fA+0, POSTINC1 ; Push X to stack
|
|
|
175 movff fA+1, POSTINC1
|
|
|
176 movff fA+2, POSTINC1
|
|
|
177 movff fA+3, POSTINC1
|
|
|
178 call log10 ; log(P0/P)
|
|
|
179
|
|
|
180 movf POSTDEC1,F,ACCESS ; pop argument
|
|
|
181 movf POSTDEC1,F,ACCESS
|
|
|
182 movf POSTDEC1,F,ACCESS
|
|
|
183 movf POSTDEC1,F,ACCESS
|
|
|
184
|
|
|
185 ; Move log10(P0/P) to fB
|
|
|
186 movff fRet+0,fB+0 ; move result to fB
|
|
|
187 movff fRet+1,fB+1
|
|
|
188 movff fRet+2,fB+2
|
|
|
189 movff fRet+3,fB+3
|
|
|
190
|
|
|
191 ; Multiply by scaling factor for meters, and standatd atmosphere.
|
|
|
192 movlw LOW(.18787)
|
|
|
193 movff WREG, fA+1
|
|
|
194 movlw HIGH(.18787)
|
|
|
195 movff WREG, fA+2
|
|
|
196 call FLO1632U ; u16 fA[1:2] --> fp32 fA
|
|
|
197 call FPM32 ; altitute --> fp32 fA
|
|
|
198
|
|
|
199 ; Convert result to int16 --> altitude.
|
|
|
200 call INT3216 ; fp32 fA --> int16 fA+1
|
|
|
201 movff fA+1, altitude+0
|
|
|
202 movff fA+2, altitude+1
|
|
|
203
|
|
|
204 return
|
|
|
205
|
|
|
206 ;=============================================================================
|
|
|
207 ; Altimeter menu
|
|
|
208 ;
|
|
|
209 ; Edit reference (where altitude = 0) pressure, while displaying corresponding
|
|
|
210 ; altitude.
|
|
|
211 ;
|
|
|
212 altimeter_menu:
|
|
|
213 call PLED_ClearScreen ; Menu header.
|
|
|
214 call PLED_standard_color
|
|
|
215 call PLED_topline_box
|
|
|
216 WIN_INVERT .1 ; Init new Wordprocessor
|
|
|
217 WIN_FONT .0
|
|
|
218 WIN_LEFT .80-7*7
|
|
|
219 WIN_TOP .0
|
|
|
220 STRCPY_PRINT "Set Altimeter:"
|
|
|
221
|
|
|
222 movlw 3 ; Start menu on line 3.
|
|
|
223 movwf menupos
|
|
|
224
|
|
|
225 altimeter_menu_2:
|
|
|
226 WIN_FONT .0 ; Reset, because compute erase that...
|
|
|
227 WIN_INVERT .0
|
|
|
228 WIN_LEFT .20 ; First line:
|
|
|
229 WIN_TOP .35
|
|
|
230 STRCPY "Sea ref:"
|
|
|
231
|
|
|
232 movff pressureRef+0, lo
|
|
|
233 movff pressureRef+1, hi
|
|
|
234 bcf STATUS,C ; Divide ref pressure by 4
|
|
|
235 rrcf hi ; to get the integer part of it:
|
|
|
236 rrcf lo
|
|
|
237 bcf STATUS,C
|
|
|
238 rrcf hi
|
|
|
239 rrcf lo
|
|
|
240 bsf leftbind
|
|
|
241 output_16
|
|
|
242
|
|
|
243 PUTC '.'
|
|
|
244 movff pressureRef+0, hi ; Decimal part is constructed
|
|
|
245 clrf WREG ; from the 2 lower bits.
|
|
|
246 btfsc hi,0
|
|
|
247 addlw .25
|
|
|
248 btfsc hi,1
|
|
|
249 addlw .50
|
|
|
250 movwf lo
|
|
|
251 output_99x
|
|
|
252
|
|
|
253 STRCAT_PRINT "mbar "
|
|
|
254
|
|
|
255 WIN_TOP .65 ; Second line:
|
|
|
256 STRCPY "Alt:"
|
|
|
257 movff altitude+0, lo
|
|
|
258 movff altitude+1, hi
|
|
|
259 bcf leftbind
|
|
|
260 output_16
|
|
|
261 STRCAT_PRINT "m "
|
|
|
262
|
|
|
263 WIN_TOP .95 ; Action enable
|
|
|
264 STRCPY "Enabled: "
|
|
|
265 GETCUSTOM8 .49
|
|
|
266 btfss WREG,0
|
|
|
267 bra alt_menu_1
|
|
|
268 STRCAT_PRINT "ON "
|
|
|
269 bra alt_menu_2
|
|
|
270 alt_menu_1:
|
|
|
271 STRCAT_PRINT "OFF"
|
|
|
272 alt_menu_2:
|
|
|
273
|
|
|
274 WIN_TOP .125 ; Action add
|
|
|
275 STRCPY_PRINT "+0.25 mbar"
|
|
|
276 WIN_TOP .155 ; Action sub
|
|
|
277 STRCPY_PRINT "-0.25 mbar"
|
|
|
278 WIN_TOP .185 ; Action exit
|
|
|
279 STRCPY_PRINT "Exit"
|
|
|
280
|
|
|
281 alt_menu_loop:
|
|
|
282 call PLED_menu_cursor ; Display cursor
|
|
|
283 bcf switch_left ; reset buttons state
|
|
|
284 bcf switch_right
|
|
|
285
|
|
|
286 alt_menu_loop1: ; Wait for button.
|
|
|
287 btfsc switch_right ; [[MENU]] button
|
|
|
288 bra alt_menu_next
|
|
|
289
|
|
|
290 btfsc switch_left ;[[ENTER]] button
|
|
|
291 bra alt_menu_do_it
|
|
|
292
|
|
|
293 btfsc divemode ; Diving stared ?
|
|
|
294 goto restart ; YES: quit this menu !
|
|
|
295
|
|
|
296 btfsc onesecupdate ; Check what should be every 1sec.
|
|
|
297 call timeout_surfmode
|
|
|
298
|
|
|
299 btfsc onesecupdate
|
|
|
300 call set_dive_modes
|
|
|
301
|
|
|
302 bcf onesecupdate ; end of 1sek. tasks
|
|
|
303
|
|
|
304 btfsc sleepmode ; Sleep mode entered ?
|
|
|
305 bra alt_menu_exit
|
|
|
306
|
|
|
307 bra alt_menu_loop1
|
|
|
308
|
|
|
309 ;---- Move to next line ------------------------------------------------------
|
|
|
310
|
|
|
311 alt_menu_next:
|
|
|
312 incf menupos ; next line.
|
|
|
313 movlw .7
|
|
|
314 cpfseq menupos ; Below last line ?
|
|
|
315 bra alt_menu_loop
|
|
|
316 movlw .3 ; Yes: back to line no 3.
|
|
|
317 movwf menupos
|
|
|
318 bra alt_menu_loop
|
|
|
319
|
|
|
320 ;----- Execute menu line -----------------------------------------------------
|
|
|
321
|
|
|
322 alt_menu_do_it:
|
|
|
323 movf menupos,W ; test line value
|
|
|
324 addlw -3
|
|
|
325 bz alt_menu_enable
|
|
|
326 dcfsnz WREG
|
|
|
327 bra alt_menu_plus1 ; 4 --> +1
|
|
|
328 dcfsnz WREG
|
|
|
329 bra alt_menu_minus1 ; 5 --> -1
|
|
|
330 bra alt_menu_exit ; else --> exit
|
|
|
331
|
|
|
332 ;---- Toggle altimeter (CF#49) -----------------------------------------------
|
|
|
333 alt_menu_enable:
|
|
|
334 GETCUSTOM8 .49 ; Read CF#49
|
|
|
335 btg WREG,0 ; Toggle boolean value
|
|
|
336 movwf EEDATA
|
|
|
337 movlw d'1' ; Upper EEPROM Bank
|
|
|
338 movwf EEADRH
|
|
|
339 movlw 4*(.49-.32) + 0x82 ; CF#49 low byte address in EEPROM
|
|
|
340 movwf EEADR
|
|
|
341 call write_eeprom
|
|
|
342 bra altimeter_menu_2
|
|
|
343
|
|
|
344 ;---- Increment sea level pressure -------------------------------------------
|
|
|
345
|
|
|
346 alt_menu_plus1:
|
|
|
347 movlb HIGH(pressureRef) ; Setup our own ram bank
|
|
|
348 infsnz pressureRef+0 ; 16bit inc.
|
|
|
349 incf pressureRef+1
|
|
|
350 bra alt_menu_recompute ; then recompute altitude.
|
|
|
351
|
|
|
352 ;---- Decrement sea level pressure -------------------------------------------
|
|
|
353
|
|
|
354 alt_menu_minus1:
|
|
|
355 movlb HIGH(pressureRef) ; Setup our own ram bank
|
|
|
356 decf pressureRef+0 ; 16bit decrement
|
|
|
357 movlw 0
|
|
|
358 subwfb pressureRef+1
|
|
|
359
|
|
|
360 alt_menu_recompute:
|
|
|
361 rcall compute_altitude ; Recompute altitude
|
|
|
362 movlb 1 ; Go back to normal bank1
|
|
|
363 bra altimeter_menu_2
|
|
|
364
|
|
|
365 ;---- Exit altimeter menu ----------------------------------------------------
|
|
|
366 alt_menu_exit:
|
|
|
367 movlw .5 ; reset position to Altimeter line.
|
|
|
368 movwf menupos ;
|
|
|
369 goto more_menu2 ; in the More... menu.
|
