Mercurial > public > mk2
annotate code_part1/OSTC_code_asm_part1/altimeter.asm @ 777:b92ee354baae
show bailout in logbook, 2.91 beta release
| author | heinrichsweikamp |
|---|---|
| date | Fri, 02 May 2014 21:40:54 +0200 |
| parents | a5d44fb115a8 |
| children |
| 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... | |
| 721 | 112 WIN_LEFT .82 |
| 113 WIN_FONT FT_SMALL | |
| 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 | |
| 721 | 120 btfss hi,7 ; Is altitude negative ? |
| 166 | 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 | |
| 721 | 133 STRCAT TXT_METER5 |
| 134 clrf WREG | |
| 135 movff WREG,letter+.11 ;limit to 12chars | |
| 136 STRCAT_PRINT "" | |
| 125 | 137 return |
| 138 | |
| 139 ;============================================================================= | |
| 140 ; Compute altitude, using the formula: | |
| 141 ; H(P) = 18.787 log10(P0/P) (Log base 10) | |
| 142 | |
| 721 | 143 ;---- Interface the the Math library ----------------------------------------- |
| 125 | 144 extern __AARGB2 ; A float in fA2, fA1, fA0, fAExo |
| 145 extern __BARGB2 ; B float in fB2, fB1, fB0, fBExo | |
| 146 extern FLO1632U ; convert uint16 fA+1 --> fp32 fA | |
| 147 extern FPD32 ; fp32 divide fA/fB --> fA | |
| 148 extern FPM32 ; fp32 multiply fA*fB --> fA | |
| 149 extern INT3216 ; convert fp32 fA --> int16 fA+1 | |
| 150 #define fA __AARGB2 | |
| 151 #define fB __BARGB2 | |
| 152 | |
| 153 ;---- Interface to the C library --------------------------------------------- | |
| 154 extern __AARGB3 | |
| 155 extern log10 ; float32 log(auto float32) | |
| 156 #define fRet __AARGB3 | |
| 157 | |
| 158 compute_altitude: | |
| 159 ; Setup C-code stack, to enable calling the log() function. | |
| 160 lfsr FSR1, c_code_data_stack | |
| 161 lfsr FSR2, c_code_data_stack | |
| 162 | |
| 163 ; Convert pressure to float, --> fB | |
| 164 movff pressureAvg+0, fA+1 | |
| 165 movff pressureAvg+1, fA+2 | |
| 166 call FLO1632U ; u16 fA[1:2] --> fp32 fA | |
| 167 movff fA+0, fB+0 | |
| 168 movff fA+1, fB+1 | |
| 169 movff fA+2, fB+2 | |
| 170 movff fA+3, fB+3 | |
| 171 | |
| 172 ; Convert sea-level reference pressure to float, --> fB | |
| 173 movff pressureRef+0, fA+1 ; Get sea level pressure. | |
| 174 movff pressureRef+1, fA+2 | |
| 175 bcf STATUS,C ; Multiply by 4. | |
| 176 rlcf fA+1 | |
| 177 rlcf fA+2 | |
| 178 bcf STATUS,C | |
| 179 rlcf fA+1 | |
| 180 rlcf fA+2 | |
| 181 call FLO1632U ; to float: u16 fA[1:2] --> fp32 fA | |
| 182 | |
| 183 ; Divide | |
| 184 call FPD32 ; fp32 X/Y --> X | |
| 185 | |
| 186 ; log10() | |
| 187 movff fA+0, POSTINC1 ; Push X to stack | |
| 188 movff fA+1, POSTINC1 | |
| 189 movff fA+2, POSTINC1 | |
| 190 movff fA+3, POSTINC1 | |
| 191 call log10 ; log(P0/P) | |
| 192 | |
| 193 movf POSTDEC1,F,ACCESS ; pop argument | |
| 194 movf POSTDEC1,F,ACCESS | |
| 195 movf POSTDEC1,F,ACCESS | |
| 196 movf POSTDEC1,F,ACCESS | |
| 197 | |
| 198 ; Move log10(P0/P) to fB | |
| 199 movff fRet+0,fB+0 ; move result to fB | |
| 200 movff fRet+1,fB+1 | |
| 201 movff fRet+2,fB+2 | |
| 202 movff fRet+3,fB+3 | |
| 203 | |
| 204 ; Multiply by scaling factor for meters, and standatd atmosphere. | |
| 205 movlw LOW(.18787) | |
| 206 movff WREG, fA+1 | |
| 207 movlw HIGH(.18787) | |
| 208 movff WREG, fA+2 | |
| 209 call FLO1632U ; u16 fA[1:2] --> fp32 fA | |
| 210 call FPM32 ; altitute --> fp32 fA | |
| 211 | |
| 212 ; Convert result to int16 --> altitude. | |
| 213 call INT3216 ; fp32 fA --> int16 fA+1 | |
| 214 movff fA+1, altitude+0 | |
| 215 movff fA+2, altitude+1 | |
| 216 | |
| 217 return | |
| 218 | |
| 219 ;============================================================================= | |
| 220 ; Altimeter menu | |
| 221 ; | |
| 222 ; Edit reference (where altitude = 0) pressure, while displaying corresponding | |
| 223 ; altitude. | |
| 224 ; | |
| 225 altimeter_menu: | |
|
132
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
226 movff pressureRef+0,WREG ; Make sure it is initialized... |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
227 movff pressureRef+1,fA |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
228 iorwf fA |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
229 bnz altimeter_menu_1 ; Yes: skip reset... |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
230 rcall altimeter_reset |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
231 |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
232 altimeter_menu_1: |
| 681 | 233 call DISP_ClearScreen ; Menu header. |
| 717 | 234 call DISP_divemask_color |
| 235 DISPLAYTEXTH .288 ; Title bar | |
| 681 | 236 call DISP_standard_color |
| 125 | 237 |
| 170 | 238 movlw 2 ; Start menu on line 2. |
| 125 | 239 movwf menupos |
| 240 | |
| 241 altimeter_menu_2: | |
| 174 | 242 WIN_FONT 0 |
| 125 | 243 WIN_LEFT .20 ; First line: |
| 244 WIN_TOP .35 | |
| 174 | 245 lfsr FSR2,letter |
| 246 OUTPUTTEXTH .289 ; Sea ref: | |
| 125 | 247 |
| 248 movff pressureRef+0, lo | |
| 249 movff pressureRef+1, hi | |
| 250 bcf STATUS,C ; Divide ref pressure by 4 | |
| 251 rrcf hi ; to get the integer part of it: | |
| 252 rrcf lo | |
| 253 bcf STATUS,C | |
| 254 rrcf hi | |
| 255 rrcf lo | |
| 256 bsf leftbind | |
| 257 output_16 | |
| 258 | |
| 445 | 259 STRCAT_PRINT TXT_MBAR7 |
| 125 | 260 |
| 170 | 261 WIN_TOP .65 ; Action enable |
| 174 | 262 lfsr FSR2, letter |
| 263 OUTPUTTEXTH .290 | |
| 125 | 264 GETCUSTOM8 .49 |
| 265 btfss WREG,0 | |
| 266 bra alt_menu_1 | |
| 199 | 267 OUTPUTTEXT .130 ; ON |
| 125 | 268 bra alt_menu_2 |
| 269 alt_menu_1: | |
| 199 | 270 OUTPUTTEXT .131 ; OFF |
| 125 | 271 alt_menu_2: |
| 199 | 272 call word_processor |
| 125 | 273 |
| 174 | 274 DISPLAYTEXTH .291 ; Action reset |
| 275 DISPLAYTEXTH .292 ; Action add | |
| 276 DISPLAYTEXTH .293 ; Action sub | |
| 277 DISPLAYTEXT .011 ; Action exit | |
| 125 | 278 |
| 721 | 279 WIN_LEFT .80 ; Bottom right. |
| 174 | 280 lfsr FSR2, letter |
| 281 OUTPUTTEXTH .294 ; "Alt: " | |
| 282 | |
| 170 | 283 movff altitude+0, lo |
| 284 movff altitude+1, hi | |
| 285 btfss hi,7 ; Is altitude negativ ? | |
| 286 bra altimeter_menu_3 ; No: just print it | |
| 287 | |
| 288 PUTC '-' ; Yes: print the minus sign | |
| 289 comf hi ; And do a 16bit 2-complement. | |
| 290 comf lo | |
| 291 infsnz lo | |
| 292 incf hi | |
| 293 | |
| 294 altimeter_menu_3: | |
| 295 bsf leftbind | |
| 296 output_16 | |
| 297 bcf leftbind | |
| 721 | 298 STRCAT TXT_METER5 |
| 299 clrf WREG | |
| 300 movff WREG,letter+.11 ;limit to 12chars | |
| 301 STRCAT_PRINT "" | |
| 170 | 302 |
| 125 | 303 alt_menu_loop: |
| 681 | 304 call DISP_menu_cursor ; Display cursor |
| 576 | 305 call wait_switches ; Waits until switches are released, resets flag if button stays pressed! |
| 125 | 306 |
| 307 alt_menu_loop1: ; Wait for button. | |
| 337 | 308 btfsc uart_dump_screen ; Asked to dump screen contains ? |
| 309 call dump_screen ; Yes! | |
| 310 | |
| 125 | 311 btfsc switch_right ; [[MENU]] button |
| 312 bra alt_menu_next | |
| 313 | |
| 314 btfsc switch_left ;[[ENTER]] button | |
| 315 bra alt_menu_do_it | |
| 316 | |
| 317 btfsc divemode ; Diving stared ? | |
| 318 goto restart ; YES: quit this menu ! | |
| 319 | |
| 320 btfsc onesecupdate ; Check what should be every 1sec. | |
| 321 call timeout_surfmode | |
| 322 | |
| 323 btfsc onesecupdate | |
| 324 call set_dive_modes | |
| 325 | |
| 326 bcf onesecupdate ; end of 1sek. tasks | |
| 327 | |
| 328 btfsc sleepmode ; Sleep mode entered ? | |
| 329 bra alt_menu_exit | |
| 330 | |
| 331 bra alt_menu_loop1 | |
| 332 | |
| 333 ;---- Move to next line ------------------------------------------------------ | |
| 334 | |
| 335 alt_menu_next: | |
| 336 incf menupos ; next line. | |
| 337 movlw .7 | |
| 338 cpfseq menupos ; Below last line ? | |
| 339 bra alt_menu_loop | |
| 170 | 340 movlw .2 ; Yes: back to line no 2. |
| 125 | 341 movwf menupos |
| 342 bra alt_menu_loop | |
| 343 | |
| 344 ;----- Execute menu line ----------------------------------------------------- | |
| 345 | |
| 346 alt_menu_do_it: | |
| 347 movf menupos,W ; test line value | |
| 170 | 348 addlw -2 |
| 349 bz alt_menu_enable ; 2 --> reset | |
| 350 dcfsnz WREG | |
| 351 bra alt_menu_reset ; 3 --> +1 | |
| 125 | 352 dcfsnz WREG |
| 353 bra alt_menu_plus1 ; 4 --> +1 | |
| 354 dcfsnz WREG | |
| 355 bra alt_menu_minus1 ; 5 --> -1 | |
| 356 bra alt_menu_exit ; else --> exit | |
| 357 | |
| 358 ;---- Toggle altimeter (CF#49) ----------------------------------------------- | |
| 359 alt_menu_enable: | |
| 360 GETCUSTOM8 .49 ; Read CF#49 | |
| 361 btg WREG,0 ; Toggle boolean value | |
| 362 movwf EEDATA | |
| 363 movlw d'1' ; Upper EEPROM Bank | |
| 364 movwf EEADRH | |
| 365 movlw 4*(.49-.32) + 0x82 ; CF#49 low byte address in EEPROM | |
| 366 movwf EEADR | |
| 367 call write_eeprom | |
|
384
904863f96582
Work-Around for "Reset all" overwriting 0x00 to 0x04 in EEPROM Bank1
heinrichsweikamp
parents:
344
diff
changeset
|
368 clrf EEADRH ; Reset EEADRH for compatibility |
| 125 | 369 bra altimeter_menu_2 |
| 370 | |
| 170 | 371 ;---- Reset sea level pressure to reference ---------------------------------- |
| 372 alt_menu_reset: | |
| 373 rcall altimeter_reset | |
| 374 bra altimeter_menu_2 | |
| 375 | |
| 125 | 376 ;---- Increment sea level pressure ------------------------------------------- |
| 377 alt_menu_plus1: | |
| 378 movlb HIGH(pressureRef) ; Setup our own ram bank | |
| 170 | 379 movlw 4 |
| 380 addwf pressureRef+0,F ; 16bit inc. | |
| 381 movlw 0 | |
| 382 addwfc pressureRef+1,F | |
| 125 | 383 bra alt_menu_recompute ; then recompute altitude. |
| 384 | |
| 385 ;---- Decrement sea level pressure ------------------------------------------- | |
| 386 alt_menu_minus1: | |
| 387 movlb HIGH(pressureRef) ; Setup our own ram bank | |
| 170 | 388 movlw -4 |
| 389 addwf pressureRef+0,F ; 16bit decrement | |
| 390 movlw -1 | |
| 391 addwfc pressureRef+1,F | |
| 125 | 392 |
| 393 alt_menu_recompute: | |
| 394 rcall compute_altitude ; Recompute altitude | |
| 395 movlb 1 ; Go back to normal bank1 | |
| 396 bra altimeter_menu_2 | |
| 397 | |
| 398 ;---- Exit altimeter menu ---------------------------------------------------- | |
| 399 alt_menu_exit: | |
| 400 movlw .5 ; reset position to Altimeter line. | |
| 401 movwf menupos ; | |
| 402 goto more_menu2 ; in the More... menu. |