Mercurial > public > mk2
annotate code_part1/OSTC_code_asm_part1/altimeter.asm @ 162:fdebc43c8372
Fix altimeter auto-reset
| author | JeanDo |
|---|---|
| date | Fri, 21 Jan 2011 01:48:40 +0100 |
| parents | 48f12271eb18 |
| children | 80de93d72a17 |
| 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 | |
| 162 | 32 movlw HIGH(4*.1100) ; Is presure ref lower than 900mbar |
| 33 cpfslt pressureRef+1 | |
| 34 bra altimeter_1 ; No: ok it is valid... | |
| 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 | |
| 40 altimeter_0: | |
| 125 | 41 movlw LOW(4*.1013+1) ; Init see level at 1013,25 mbar. |
| 42 movwf pressureRef+0 | |
| 43 movlw HIGH(4*.1013+1) | |
| 44 movwf pressureRef+1 | |
| 45 | |
| 46 ; Reset computation. Eg. after a sleep, enables to faster restart with correct | |
| 47 ; values... | |
| 48 altimeter_reset: | |
| 49 movlb HIGH(pressureAvg) | |
| 50 clrf pressureSum+0 ; Init averaging area | |
| 51 clrf pressureSum+1 | |
| 52 clrf pressureCount | |
| 53 | |
| 54 movff amb_pressure+0,pressureAvg+0 ; And init first average. | |
| 55 movff amb_pressure+1,pressureAvg+1 | |
| 56 | |
| 57 movlw 4 ; And multiply AVG by 16 to be coherent. | |
| 58 altimeter_reset_1: | |
| 59 bcf STATUS,C | |
| 60 rlcf pressureAvg+0 | |
| 61 rlcf pressureAvg+1 | |
| 62 decfsz WREG | |
| 63 bra altimeter_reset_1 | |
|
132
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
64 |
|
49bb155ddfbf
Fix altimeter after simulator (zero divide -> 32767m)
JeanDo
parents:
125
diff
changeset
|
65 rcall compute_altitude |
| 125 | 66 |
| 67 movlb 1 ; Back to normal bank1. | |
| 68 return | |
| 69 | |
| 70 altimeter_1: | |
| 71 ;---- Do a bank-safe 16bit summing ----------------------------------- | |
| 72 movff amb_pressure+0,WREG | |
| 73 addwf pressureSum+0,F | |
| 74 movff amb_pressure+1,WREG | |
| 75 addwfc pressureSum+1,F | |
| 76 | |
| 77 incf pressureCount ; Increment count too. | |
| 78 movlw .32 ; Already 32 done ? | |
| 79 subwf pressureCount,W | |
| 80 bnz altimeter_4 ; NO: skip update. | |
| 81 | |
| 82 ;---- Update altitude every 32 pressure measures -------------------- | |
| 83 bcf STATUS,C ; Divide by 2, to store pressure x16 | |
| 84 rrcf pressureSum+1 | |
| 85 rrcf pressureSum+0 | |
| 86 | |
| 87 movff pressureSum+0,pressureAvg+0 | |
| 88 movff pressureSum+1,pressureAvg+1 | |
| 89 | |
| 90 rcall compute_altitude ; Compute from the averaged value... | |
| 91 | |
| 92 clrf pressureSum+0 ; And reset sum zone for next averaging. | |
| 93 clrf pressureSum+1 | |
| 94 clrf pressureCount | |
| 95 | |
| 96 altimeter_4: | |
| 97 movlb 1 ; make sure to be in normal bank1 | |
| 98 return | |
| 99 | |
| 100 ;---- Display result ------------------------------------------------- | |
| 101 altimeter_display: | |
| 102 GETCUSTOM8 .49 ; Check CF#49 | |
| 103 btfss WREG,0 ; Enabled ? | |
| 104 return ; NO: return | |
| 105 | |
| 106 WIN_TOP .35 ; Custom view drawing zone... | |
| 107 WIN_LEFT .90 | |
| 108 WIN_INVERT .0 | |
| 109 WIN_FONT .0 | |
| 110 call PLED_standard_color | |
| 111 | |
| 112 STRCPY "Alt:" | |
| 113 | |
| 114 movff altitude+0,lo ; BANK-SAFE read altitude | |
| 115 movff altitude+1,hi | |
| 116 movf lo,W ; Is it zero (not computed yet) ? | |
| 117 iorwf hi,W | |
| 118 bz altimeter_2 | |
| 119 | |
| 120 bsf leftbind | |
| 121 output_16 | |
| 122 bcf leftbind | |
| 123 bra altimeter_3 | |
| 124 | |
| 125 altimeter_2: | |
| 126 STRCAT "****" | |
| 127 | |
| 128 altimeter_3: | |
| 129 STRCAT_PRINT "m " | |
| 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 | |
| 230 WIN_FONT .0 | |
| 231 WIN_LEFT .80-7*7 | |
| 134 | 232 WIN_TOP .1 |
| 125 | 233 STRCPY_PRINT "Set Altimeter:" |
| 234 | |
| 235 movlw 3 ; Start menu on line 3. | |
| 236 movwf menupos | |
| 237 | |
| 238 altimeter_menu_2: | |
| 239 WIN_FONT .0 ; Reset, because compute erase that... | |
| 240 WIN_INVERT .0 | |
| 241 WIN_LEFT .20 ; First line: | |
| 242 WIN_TOP .35 | |
| 243 STRCPY "Sea ref:" | |
| 244 | |
| 245 movff pressureRef+0, lo | |
| 246 movff pressureRef+1, hi | |
| 247 bcf STATUS,C ; Divide ref pressure by 4 | |
| 248 rrcf hi ; to get the integer part of it: | |
| 249 rrcf lo | |
| 250 bcf STATUS,C | |
| 251 rrcf hi | |
| 252 rrcf lo | |
| 253 bsf leftbind | |
| 254 output_16 | |
| 255 | |
| 256 PUTC '.' | |
| 257 movff pressureRef+0, hi ; Decimal part is constructed | |
| 258 clrf WREG ; from the 2 lower bits. | |
| 259 btfsc hi,0 | |
| 260 addlw .25 | |
| 261 btfsc hi,1 | |
| 262 addlw .50 | |
| 263 movwf lo | |
| 264 output_99x | |
| 265 | |
| 266 STRCAT_PRINT "mbar " | |
| 267 | |
| 268 WIN_TOP .65 ; Second line: | |
| 269 STRCPY "Alt:" | |
| 270 movff altitude+0, lo | |
| 271 movff altitude+1, hi | |
| 272 bcf leftbind | |
| 273 output_16 | |
| 274 STRCAT_PRINT "m " | |
| 275 | |
| 276 WIN_TOP .95 ; Action enable | |
| 277 STRCPY "Enabled: " | |
| 278 GETCUSTOM8 .49 | |
| 279 btfss WREG,0 | |
| 280 bra alt_menu_1 | |
| 281 STRCAT_PRINT "ON " | |
| 282 bra alt_menu_2 | |
| 283 alt_menu_1: | |
| 284 STRCAT_PRINT "OFF" | |
| 285 alt_menu_2: | |
| 286 | |
| 287 WIN_TOP .125 ; Action add | |
| 288 STRCPY_PRINT "+0.25 mbar" | |
| 289 WIN_TOP .155 ; Action sub | |
| 290 STRCPY_PRINT "-0.25 mbar" | |
| 291 WIN_TOP .185 ; Action exit | |
| 292 STRCPY_PRINT "Exit" | |
| 293 | |
| 294 alt_menu_loop: | |
| 295 call PLED_menu_cursor ; Display cursor | |
| 296 bcf switch_left ; reset buttons state | |
| 297 bcf switch_right | |
| 298 | |
| 299 alt_menu_loop1: ; Wait for button. | |
| 300 btfsc switch_right ; [[MENU]] button | |
| 301 bra alt_menu_next | |
| 302 | |
| 303 btfsc switch_left ;[[ENTER]] button | |
| 304 bra alt_menu_do_it | |
| 305 | |
| 306 btfsc divemode ; Diving stared ? | |
| 307 goto restart ; YES: quit this menu ! | |
| 308 | |
| 309 btfsc onesecupdate ; Check what should be every 1sec. | |
| 310 call timeout_surfmode | |
| 311 | |
| 312 btfsc onesecupdate | |
| 313 call set_dive_modes | |
| 314 | |
| 315 bcf onesecupdate ; end of 1sek. tasks | |
| 316 | |
| 317 btfsc sleepmode ; Sleep mode entered ? | |
| 318 bra alt_menu_exit | |
| 319 | |
| 320 bra alt_menu_loop1 | |
| 321 | |
| 322 ;---- Move to next line ------------------------------------------------------ | |
| 323 | |
| 324 alt_menu_next: | |
| 325 incf menupos ; next line. | |
| 326 movlw .7 | |
| 327 cpfseq menupos ; Below last line ? | |
| 328 bra alt_menu_loop | |
| 329 movlw .3 ; Yes: back to line no 3. | |
| 330 movwf menupos | |
| 331 bra alt_menu_loop | |
| 332 | |
| 333 ;----- Execute menu line ----------------------------------------------------- | |
| 334 | |
| 335 alt_menu_do_it: | |
| 336 movf menupos,W ; test line value | |
| 337 addlw -3 | |
| 338 bz alt_menu_enable | |
| 339 dcfsnz WREG | |
| 340 bra alt_menu_plus1 ; 4 --> +1 | |
| 341 dcfsnz WREG | |
| 342 bra alt_menu_minus1 ; 5 --> -1 | |
| 343 bra alt_menu_exit ; else --> exit | |
| 344 | |
| 345 ;---- Toggle altimeter (CF#49) ----------------------------------------------- | |
| 346 alt_menu_enable: | |
| 347 GETCUSTOM8 .49 ; Read CF#49 | |
| 348 btg WREG,0 ; Toggle boolean value | |
| 349 movwf EEDATA | |
| 350 movlw d'1' ; Upper EEPROM Bank | |
| 351 movwf EEADRH | |
| 352 movlw 4*(.49-.32) + 0x82 ; CF#49 low byte address in EEPROM | |
| 353 movwf EEADR | |
| 354 call write_eeprom | |
| 355 bra altimeter_menu_2 | |
| 356 | |
| 357 ;---- Increment sea level pressure ------------------------------------------- | |
| 358 | |
| 359 alt_menu_plus1: | |
| 360 movlb HIGH(pressureRef) ; Setup our own ram bank | |
| 361 infsnz pressureRef+0 ; 16bit inc. | |
| 362 incf pressureRef+1 | |
| 363 bra alt_menu_recompute ; then recompute altitude. | |
| 364 | |
| 365 ;---- Decrement sea level pressure ------------------------------------------- | |
| 366 | |
| 367 alt_menu_minus1: | |
| 368 movlb HIGH(pressureRef) ; Setup our own ram bank | |
| 369 decf pressureRef+0 ; 16bit decrement | |
| 370 movlw 0 | |
| 371 subwfb pressureRef+1 | |
| 372 | |
| 373 alt_menu_recompute: | |
| 374 rcall compute_altitude ; Recompute altitude | |
| 375 movlb 1 ; Go back to normal bank1 | |
| 376 bra altimeter_menu_2 | |
| 377 | |
| 378 ;---- Exit altimeter menu ---------------------------------------------------- | |
| 379 alt_menu_exit: | |
| 380 movlw .5 ; reset position to Altimeter line. | |
| 381 movwf menupos ; | |
| 382 goto more_menu2 ; in the More... menu. |