Mercurial > public > hwos_code
annotate src/compass_ops.asm @ 645:070528a88715
3.16 release
| author | heinrichsweikamp |
|---|---|
| date | Sun, 07 Nov 2021 12:39:23 +0100 |
| parents | 7d8a4c60ec1a |
| children | ef2ed7e3a895 |
| rev | line source |
|---|---|
| 582 | 1 ;============================================================================= |
| 2 ; | |
| 634 | 3 ; File compass_ops.asm combined next generation V3.09.5 |
| 582 | 4 ; |
| 5 ; Compass Operations | |
| 6 ; | |
| 7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved. | |
| 8 ;============================================================================= | |
| 9 | |
| 634 | 10 #include "hwos.inc" |
| 11 #include "i2c.inc" | |
| 12 #include "tft_outputs.inc" | |
| 13 #include "tft.inc" | |
| 14 #include "strings.inc" | |
| 15 #include "wait.inc" | |
| 16 #include "surfmode.inc" | |
| 17 #include "divemode.inc" | |
| 18 #include "math.inc" | |
| 19 #include "convert.inc" | |
| 20 #include "start.inc" | |
| 21 #include "colorschemes.inc" | |
| 147 | 22 |
| 0 | 23 |
| 623 | 24 IFDEF _compass |
| 25 | |
| 26 | |
| 27 ; local flags | |
| 28 #DEFINE compass_show_cardinal compass_flags,0 ; =1: show the cardinal (N, NE, E, ...) | |
| 29 #DEFINE compass_bearing_eq compass_flags,1 ; =1: bearing is in direction, do not show << or >> | |
| 30 #DEFINE compass_bearing_lft compass_flags,2 ; =1: bearing is to the left/<<, =0: to the right/>> | |
| 31 #DEFINE compass_bearing_vis compass_flags,3 ; =1: bearing is visible (either ahead or behind/-180°) | |
| 32 #DEFINE compass_bearing_ahd compass_flags,4 ; =1: bearing is ahead, =0: behind | |
| 33 ; compass_flags,5 ; --- unused | |
| 34 ; compass_flags,6 ; --- unused | |
| 35 ; compass_flags,7 ; --- unused | |
| 36 | |
| 37 | |
| 0 | 38 ; Make sure symbols from the .inc are available to the C code: |
| 623 | 39 |
| 40 ; filtered data - Compass | |
| 582 | 41 global compass_DX_f |
| 42 global compass_DY_f | |
| 43 global compass_DZ_f | |
| 0 | 44 |
| 623 | 45 ; filtered Data - Accelerometer |
| 582 | 46 global accel_DX_f |
| 47 global accel_DY_f | |
| 48 global accel_DZ_f | |
| 49 | |
| 623 | 50 ; Calibration Data |
| 582 | 51 global compass_CX_f |
| 52 global compass_CY_f | |
| 53 global compass_CZ_f | |
| 0 | 54 |
| 623 | 55 ; Temporary Values to pass Q15 Arithmetics around |
| 582 | 56 global compass_a |
| 57 global compass_b | |
| 0 | 58 |
| 582 | 59 ; Result |
| 623 | 60 global compass_heading_new |
| 0 | 61 |
| 147 | 62 |
| 582 | 63 extern compass |
| 64 extern compass_reset_calibration | |
| 65 extern compass_add_calibration | |
| 66 extern compass_solve_calibration | |
| 623 | 67 |
| 631 | 68 extern option_check_and_store_all |
| 582 | 69 |
|
337
508d7fb98b34
cleanup menu, add "Auto SP" option (Not working yet), minor layout change in compass menu
heinrichsweikamp
parents:
300
diff
changeset
|
70 |
| 634 | 71 ;============================================================================= |
| 72 compass_ops1 CODE | |
| 73 ;============================================================================= | |
| 582 | 74 |
| 75 | |
| 0 | 76 ;----------------------------------------------------------------------------- |
| 77 ; Filter compass values | |
| 78 ; | |
| 79 ; Apply linear filtering to input parameters. | |
| 80 | |
| 582 | 81 ; Apply filtering formula: reg_f += (reg - reg_f) / 4 |
| 82 FILTER16 MACRO reg, reg_f | |
| 83 movf reg_f+0,W | |
| 84 subwf reg+0,W | |
| 85 movwf PRODL | |
| 86 movf reg_f+1,W | |
| 87 subwfb reg+1,W | |
| 88 rcall filter_16_common | |
| 89 addwf reg_f+0,F | |
| 90 movf PRODH,W | |
| 91 addwfc reg_f+1,F | |
| 92 ENDM | |
| 148 | 93 |
| 94 filter_16_common: | |
| 582 | 95 movwf PRODH |
| 623 | 96 bcf STATUS,C ; copy sign bit into carry |
| 582 | 97 btfsc PRODH,7 |
| 98 bsf STATUS,C | |
| 623 | 99 rrcf PRODH,F ; 16 bit shift right |
| 582 | 100 rrcf PRODL,F |
| 623 | 101 bcf STATUS,C ; copy sign bit into carry |
| 582 | 102 btfsc PRODH,7 |
| 103 bsf STATUS,C | |
| 623 | 104 rrcf PRODH,F ; 16 bit shift right |
| 582 | 105 rrcf PRODL,W |
| 106 return | |
| 0 | 107 |
| 623 | 108 |
| 582 | 109 global compass_filter |
| 0 | 110 compass_filter: |
| 623 | 111 banksel compass_DX ; select bank common2 |
| 582 | 112 FILTER16 compass_DX, compass_DX_f |
| 113 FILTER16 compass_DY, compass_DY_f | |
| 114 FILTER16 compass_DZ, compass_DZ_f | |
| 115 FILTER16 accel_DX, accel_DX_f | |
| 116 FILTER16 accel_DY, accel_DY_f | |
| 117 FILTER16 accel_DZ, accel_DZ_f | |
| 623 | 118 banksel common ; back to bank common |
| 582 | 119 return |
| 0 | 120 |
| 121 | |
| 634 | 122 ;============================================================================= |
| 123 compass_ops2 CODE | |
| 124 ;============================================================================= | |
| 0 | 125 |
| 126 ;----------------------------------------------------------------------------- | |
| 127 ; Q15 fractional numbers: a * b / 2**16 (UNSIGNED) | |
| 128 ; | |
| 623 | 129 ; Uses 16x16->16 multiply, for positive integers, keeping only the most |
| 130 ; relevant bits. | |
| 0 | 131 ; |
| 132 ; Used to multiply two Q15 numbers, in the range 0..1, | |
| 133 ; represented as 0..32767, that is a / 2**15. | |
| 134 ; | |
| 135 ; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14. | |
| 136 ; So to get back a Q15 number, we need a shift-left... | |
| 623 | 137 |
| 138 global compass_umul ; called from compass.c | |
| 0 | 139 compass_umul: |
| 623 | 140 banksel compass_a ; select bank common2 |
| 582 | 141 rcall compass_mul_16 |
| 0 | 142 |
| 143 ; The 2x time, by left-shifting inserting the missing bit: | |
| 144 compass_mul_2: | |
| 623 | 145 rlcf compass_r+2,F ; missing bit into carry |
| 582 | 146 rlcf compass_r+0,F |
| 147 rlcf compass_r+1,F | |
| 623 | 148 MOVII compass_r,PROD ; return value into PROD |
| 582 | 149 return |
| 0 | 150 |
| 151 ; The 16x16-> multiply: | |
| 152 compass_mul_16: | |
| 623 | 153 movf compass_a+1,W ; block ah*bh |
| 582 | 154 mulwf compass_b+1 |
| 155 movff PRODL,compass_r+0 ; and copy | |
| 156 movff PRODH,compass_r+1 | |
| 0 | 157 |
| 623 | 158 movf compass_a+0,W ; block al*bl |
| 159 mulwf compass_b+0 | |
| 160 movff PRODH,compass_r+2 ; into fraction byte | |
| 0 | 161 |
| 623 | 162 movf compass_a+1,W ; block ah*bl |
| 582 | 163 mulwf compass_b+0 |
| 164 movf PRODL,W | |
| 623 | 165 addwf compass_r+2,F ; fraction part to carry |
| 582 | 166 movf PRODH,W ; and add16 |
| 167 addwfc compass_r+0,F | |
| 168 movlw 0 | |
| 628 | 169 addwfc compass_r+1,F |
| 0 | 170 |
| 623 | 171 movf compass_a+0,W ; block al*bh |
| 582 | 172 mulwf compass_b+1 |
| 173 movf PRODL,W | |
| 623 | 174 addwf compass_r+2,F ; fraction part to carry |
| 582 | 175 movf PRODH,W ; and add16 |
| 176 addwfc compass_r+0,F | |
| 177 movlw 0 | |
| 178 addwfc compass_r+1,F | |
| 0 | 179 |
| 582 | 180 return |
| 0 | 181 |
| 182 ;----------------------------------------------------------------------------- | |
| 183 ; Q15 fractional numbers: a * b / 2**16 (SIGNED) | |
| 184 | |
| 623 | 185 global compass_imul ; called from compass.c |
| 0 | 186 compass_imul: |
| 623 | 187 banksel compass_a ; select bank common2 |
| 582 | 188 rcall compass_mul_16 |
| 0 | 189 |
| 582 | 190 btfss compass_b+1,7 |
| 191 bra compass_mul_3 | |
| 0 | 192 |
| 582 | 193 movf compass_a+0,W |
| 194 subwf compass_r+0,F | |
| 195 movf compass_a+1,W | |
| 196 subwfb compass_r+1,F | |
| 0 | 197 |
| 198 compass_mul_3: | |
| 582 | 199 btfss compass_a+1,7 |
| 200 bra compass_mul_4 | |
| 0 | 201 |
| 582 | 202 movf compass_b+0,W |
| 203 subwf compass_r+0,F | |
| 204 movf compass_b+1,W | |
| 623 | 205 subwfb compass_r+1,F |
| 0 | 206 |
| 207 compass_mul_4: | |
| 628 | 208 bcf compass_r+1,6 ; copy bit 7 to 6, so keep it after 2x |
| 582 | 209 btfsc compass_r+1,7 |
| 210 bsf compass_r+1,6 | |
| 211 bra compass_mul_2 | |
| 0 | 212 |
| 623 | 213 |
| 634 | 214 ;============================================================================= |
| 215 compass_ops3 CODE | |
| 216 ;============================================================================= | |
| 217 | |
| 218 global compass_calibration | |
| 219 compass_calibration: ; compass calibration | |
| 628 | 220 bsf block_sensor_interrupt ; disable sensor interrupts |
| 221 call I2C_sleep_compass ; stop compass | |
| 634 | 222 call TFT_ClearScreen ; clear the screen |
| 582 | 223 ; Mask |
| 634 | 224 WIN_STD .16,.0 ; set position |
| 225 FONT_COLOR color_greenish ; set font color | |
| 226 STRCPY_TEXT_PRINT tCompassCalibration | |
| 227 btfss switch_right2 ; wait until button (with a moving magnet in some OSTC!) is released | |
| 623 | 228 bra $-2 |
| 147 | 229 |
| 582 | 230 ; WIN_SMALL .0,.215 |
| 634 | 231 ; FONT_COLOR color_standard |
| 582 | 232 ; STRCPY_TEXT_PRINT tExit |
| 233 WAITMS d'255' | |
| 234 WAITMS d'255' | |
| 147 | 235 |
| 628 | 236 call request_speed_fastest ; request CPU speed change to fastest speed |
| 623 | 237 |
| 643 | 238 movlw .5 ; initialize gain |
| 634 | 239 movff WREG,opt_compass_gain ; ... |
| 428 | 240 |
| 628 | 241 movlw .60 ; initialize timeout to 60 seconds |
| 242 movwf isr_timeout_reload ; copy WREG to isr_timeout_reload | |
| 634 | 243 bsf restart_timeout ; request ISR to restart the timeout |
| 628 | 244 bcf trigger_timeout ; clear any pending timeout trigger |
| 245 compass_calibration_gainset: ; reduce the gain, set bank here! | |
| 246 banksel opt_compass_gain ; select bank options table | |
| 247 decf opt_compass_gain,F ; reduce by one | |
| 248 btfsc STATUS,N ; < 0 ? | |
| 249 clrf opt_compass_gain ; YES - keep at zero | |
| 250 btfsc STATUS,N ; < 0 ? | |
| 634 | 251 bra compass_calibration_loop1 ; YES - skip gain stuff (Would hang here in case of compass failure) |
| 628 | 252 banksel common ; bank to bank common |
| 623 | 253 |
| 634 | 254 call I2C_init_compass ; initialize compass chip |
| 582 | 255 |
| 628 | 256 btfsc compass_type3 ; compass type 3 ? |
| 257 bra compass_calibration_loop1 ; YES - skip gain stuff | |
| 258 btfsc compass_type2 ; compass type 2 ? | |
| 259 bra compass_calibration_loop1 ; YES - skip gain stuff | |
| 582 | 260 |
| 628 | 261 rcall TFT_compass_show_gain ; show the current compass gain |
| 147 | 262 |
| 582 | 263 WAITMS d'250' |
| 628 | 264 WAITMS d'250' ; wait for first reading... |
| 147 | 265 |
| 628 | 266 movlw .60 ; calibration shall run for 60 seconds |
| 634 | 267 call restart_timeout_time ; restart the timeout |
| 147 | 268 |
| 628 | 269 call I2C_RX_compass ; read compass |
| 270 call I2C_RX_accelerometer ; read accelerometer | |
| 147 | 271 |
| 582 | 272 ; Test all axes for +4096 (Hi byte=16) |
| 628 | 273 banksel compass_DX ; select bank common2 |
| 582 | 274 movlw .16 |
| 275 cpfseq compass_DX+1 | |
| 276 bra $+4 | |
| 277 bra compass_calibration_gainset | |
| 278 cpfseq compass_DY+1 | |
| 279 bra $+4 | |
| 280 bra compass_calibration_gainset | |
| 281 cpfseq compass_DZ+1 | |
| 282 bra $+4 | |
| 283 bra compass_calibration_gainset | |
| 147 | 284 |
| 582 | 285 ; Test all axes for -4096 (Hi byte=240) |
| 286 movlw .240 | |
| 287 cpfseq compass_DX+1 | |
| 288 bra $+4 | |
| 289 bra compass_calibration_gainset | |
| 290 cpfseq compass_DY+1 | |
| 291 bra $+4 | |
| 292 bra compass_calibration_gainset | |
| 293 cpfseq compass_DZ+1 | |
| 294 bra $+4 | |
| 295 bra compass_calibration_gainset | |
| 147 | 296 |
| 628 | 297 compass_calibration_loop1: ; done with gain |
| 298 banksel common ; bank to bank common | |
| 299 rcall compass_filter_init ; set DX_f values | |
| 300 call compass_reset_calibration ; reset CX_f values (C-code) | |
| 301 banksel common ; back to bank common | |
| 147 | 302 |
| 303 compass_calibration_loop2: | |
| 628 | 304 call I2C_RX_compass ; read compass |
| 305 call I2C_RX_accelerometer ; test accelerometer | |
| 634 | 306 call compass_filter ; filter compass raw data |
| 147 | 307 |
| 582 | 308 ; Twice |
| 628 | 309 call I2C_RX_compass ; read compass |
| 310 call I2C_RX_accelerometer ; test accelerometer | |
| 634 | 311 call compass_filter ; filter compass raw data |
| 582 | 312 |
| 628 | 313 ; btfsc compass_type1 ; compass1? |
| 314 ; bra compass_calibration_loop3 ; YES - skip gain stuff | |
| 147 | 315 |
| 645 | 316 banksel opt_compass_gain ; select bank options table |
| 317 tstfsz opt_compass_gain ; =0? | |
| 318 bra $+4 ; No | |
| 319 bra compass_calibration_loop3 ; YES - skip gain stuff | |
| 582 | 320 ; Test all axes for +4096 (Hi byte=16) |
| 628 | 321 banksel compass_DX ; select bank common2 |
| 582 | 322 movlw .16 |
| 323 cpfseq compass_DX+1 | |
| 324 bra $+4 | |
| 325 bra compass_calibration_gainset | |
| 326 cpfseq compass_DY+1 | |
| 327 bra $+4 | |
| 328 bra compass_calibration_gainset | |
| 329 cpfseq compass_DZ+1 | |
| 330 bra $+4 | |
| 331 bra compass_calibration_gainset | |
| 147 | 332 |
| 582 | 333 ; Test all axes for -4096 (Hi byte=240) |
| 334 movlw .240 | |
| 335 cpfseq compass_DX+1 | |
| 336 bra $+4 | |
| 337 bra compass_calibration_gainset | |
| 338 cpfseq compass_DY+1 | |
| 339 bra $+4 | |
| 340 bra compass_calibration_gainset | |
| 341 cpfseq compass_DZ+1 | |
| 342 bra $+4 | |
| 343 bra compass_calibration_gainset | |
| 147 | 344 |
| 428 | 345 compass_calibration_loop3: |
| 645 | 346 banksel common ; back to bank common |
| 623 | 347 ; and register only one value out of four: |
| 628 | 348 call compass_add_calibration ; check and store new max/min values (C-code) |
| 349 banksel common ; back to bank common | |
| 147 | 350 |
| 628 | 351 rcall TFT_compass_fast ; show values |
| 352 btfsc trigger_timeout ; timeout (calibration done)? | |
| 353 bra compass_calibration_exit ; YES - done | |
| 354 btfss trigger_full_second ; NO - new second begun? | |
| 631 | 355 bra compass_calibration_loop2 ; NO - loop |
| 628 | 356 bcf trigger_full_second ; YES - clear flag |
| 357 rcall TFT_show_timeout_testmode ; - show remaining time | |
| 358 bra compass_calibration_loop2 ; - loop | |
| 147 | 359 |
| 360 compass_calibration_exit: | |
| 628 | 361 bcf block_sensor_interrupt ; re-enable sensor interrupts |
| 623 | 362 |
| 628 | 363 call compass_solve_calibration ; calculate calibration factors (C-code) |
| 364 banksel common ; back to bank common | |
| 365 | |
| 366 call request_speed_normal ; request CPU speed change to normal speed | |
| 623 | 367 |
| 634 | 368 bsf option_changed ; flag that option values have changed |
| 631 | 369 bsf restart_fast ; request to skip logos and waits on restart |
| 370 | |
| 371 movlw .6 ; coding for surface compass view | |
| 372 movff WREG,customview_surfmode ; set to compass view to show | |
| 373 | |
| 374 goto restart ; done | |
| 623 | 375 |
| 147 | 376 |
| 634 | 377 ;----------------------------------------------------------------------------- |
| 378 ; Helper Function | |
| 379 ; | |
| 380 compass_filter_init: | |
| 381 MOVII compass_DX,compass_DX_f | |
| 382 MOVII compass_DY,compass_DY_f | |
| 383 MOVII compass_DZ,compass_DZ_f | |
| 628 | 384 |
| 634 | 385 MOVII accel_DX,accel_DX_f |
| 386 MOVII accel_DY,accel_DY_f | |
| 387 MOVII accel_DZ,accel_DZ_f | |
| 582 | 388 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
389 |
| 634 | 390 ;----------------------------------------------------------------------------- |
| 391 ; Show Calibration Progress | |
| 392 ; | |
| 393 TFT_compass_fast: | |
| 394 WIN_TINY .0,.50 ; set font and position | |
| 395 FONT_COLOR_MEMO ; set font color | |
| 396 | |
| 397 STRCPY "Cx:" ; print label | |
| 398 MOVII compass_DX,mpr ; get value | |
| 399 rcall TFT_compass_fast_helper ; print value | |
| 400 | |
| 401 STRCAT " Cy:" ; print label | |
| 402 MOVII compass_DY,mpr ; get value | |
| 403 rcall TFT_compass_fast_helper ; print value | |
| 404 | |
| 405 STRCAT " Cz:" ; print label | |
| 406 MOVII compass_DZ,mpr ; get value | |
| 407 rcall TFT_compass_fast_helper ; print value | |
| 408 | |
| 409 PRINT ; dump to screen | |
| 410 | |
| 411 WIN_TINY .0,.104 ; set font and position | |
| 412 FONT_COLOR_MEMO ; set font color | |
| 413 | |
| 414 STRCPY "Ax:" ; print label | |
| 415 MOVII accel_DX,mpr ; get value | |
| 416 rcall TFT_compass_fast_helper ; print value | |
| 417 | |
| 418 STRCAT " Ay:" ; print label | |
| 419 MOVII accel_DY,mpr ; get value | |
| 420 rcall TFT_compass_fast_helper ; print value | |
| 421 | |
| 422 STRCAT " Az:" ; print label | |
| 423 MOVII accel_DZ,mpr ; get value | |
| 424 rcall TFT_compass_fast_helper ; print value | |
| 425 | |
| 426 PRINT ; dump to screen | |
| 427 return ; done | |
| 428 | |
| 429 | |
| 430 TFT_compass_fast_helper: | |
| 431 call convert_signed_16bit ; convert lo:hi into unsigned-short and add '-' to POSTINC2 if required | |
| 432 btfsc neg_flag ; is value negative? | |
| 433 bra TFT_compass_fast_helper1; YES - print value | |
| 434 PUTC '+' ; NO - print a plus sign first | |
| 435 TFT_compass_fast_helper1: | |
| 436 output_65535 ; print value (0-65535) | |
| 582 | 437 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
438 |
| 634 | 439 |
| 440 | |
| 441 ;----------------------------------------------------------------------------- | |
| 442 ; Show remaining Calibration Time | |
| 443 ; | |
| 444 TFT_show_timeout_testmode: | |
| 445 WIN_TINY .0,.68 ; set font and position | |
| 446 FONT_COLOR_MEMO ; set font color | |
| 447 STRCPY "T:" ; print label | |
| 448 movff isr_timeout_timer,lo ; get remaining time | |
| 449 output_256 ; print remaining time (xxx) | |
| 450 PUTC_PRINT "s" ; append unit and dump to screen | |
| 451 return ; done | |
| 452 | |
| 453 | |
| 454 ;----------------------------------------------------------------------------- | |
| 455 ; Show current Compass Gain | |
| 456 ; | |
| 628 | 457 TFT_compass_show_gain: ; show the current compass gain |
| 458 ; movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) | |
| 582 | 459 ; tstfsz lo |
| 634 | 460 ; return ; do not show unless gain > 0 |
| 461 WIN_TINY .0,.86 ; set font and position | |
| 462 FONT_COLOR_MEMO ; set font color | |
| 463 STRCPY_TEXT tCompassGain ; print label | |
| 464 movff opt_compass_gain,lo ; get value: 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) | |
| 465 output_99 ; print value (xx) | |
| 466 PRINT ; dump to screen | |
| 467 return ; done | |
| 468 | |
| 469 | |
| 470 ;============================================================================= | |
| 471 compass_ops4 CODE | |
| 472 ;============================================================================= | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
473 |
| 628 | 474 ;----------------------------------------------------------------------------- |
| 634 | 475 ; Mask for Compass in Surface Mode |
| 476 ; | |
| 582 | 477 global TFT_surface_compass_mask |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
478 TFT_surface_compass_mask: |
| 582 | 479 WIN_SMALL surf_compass_mask_column,surf_compass_mask_row |
| 634 | 480 FONT_COLOR_MEMO ; set font color |
| 628 | 481 STRCPY_TEXT_PRINT tHeading ; print "Heading:" |
| 634 | 482 return ; done |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
483 |
| 623 | 484 |
| 634 | 485 ;----------------------------------------------------------------------------- |
| 486 ; Compass Display in Surface Mode | |
| 487 ; | |
| 582 | 488 global TFT_surface_compass_heading |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
489 TFT_surface_compass_heading: |
| 634 | 490 call compass_heading_common ; compute heading |
| 582 | 491 WIN_STD surf_compass_head_column,surf_compass_head_row |
| 634 | 492 |
| 628 | 493 TFT_surface_compass_heading_com: ; show "000° N" |
| 634 | 494 FONT_COLOR_MEMO ; set default / dive-mode standard color |
| 628 | 495 movff compass_heading_new+1,WREG ; get upper byte of actual heading |
| 496 btfsc WREG,7 ; compass calibrated? | |
| 497 bra TFT_compass_uncalibrated ; NO | |
| 498 MOVII compass_heading_shown,mpr ; get heading to be shown | |
| 499 rcall TFT_compass_helper ; show heading and its cardinal | |
| 500 btfsc divemode ; in dive mode? | |
| 501 return ; YES - done for dive mode | |
| 502 ; in surface mode - shall show bearing? | |
| 503 btfss compass_bearing_set ; is a bearing set? | |
| 504 return ; NO - done | |
| 505 btfsc compass_menu ; is the "set bearing" selection shown? | |
| 506 return ; YES - done | |
| 507 ; show bearing | |
| 508 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row | |
| 634 | 509 FONT_COLOR color_yellow ; set font color |
| 628 | 510 MOVII compass_bearing,mpr ; get bearing |
| 511 ;bra TFT_compass_helper ; show number and cardinal and return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
512 |
| 628 | 513 TFT_compass_helper: |
| 634 | 514 output_999 ; print bearing |
| 515 STRCAT "° " ; append unit and a space | |
| 516 call tft_compass_cardinal ; append cardinal buffer | |
| 517 PRINT ; dump to screen | |
| 628 | 518 return ; done |
| 519 | |
| 520 TFT_compass_uncalibrated: | |
| 521 STRCAT_PRINT "---°" ; print "---°" | |
| 522 return ; done | |
| 382 | 523 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
524 |
| 634 | 525 ;============================================================================= |
| 526 compass_ops5 CODE | |
| 527 ;============================================================================= | |
| 528 | |
| 529 | |
| 530 ;----------------------------------------------------------------------------- | |
| 531 ; Mask for Compass in Dive Mode | |
| 532 ; | |
| 533 global TFT_dive_compass_mask ; draws the white box around the heading tape | |
| 534 TFT_dive_compass_mask: | |
| 535 WIN_FRAME_STD dm_custom_compass_graph_row, dm_custom_compass_graph_row+dm_custom_compass_graph_height, .0, .159 | |
| 536 return | |
| 537 | |
| 538 | |
| 539 ;----------------------------------------------------------------------------- | |
| 540 ; Compass Display in Dive Mode | |
| 541 ; | |
| 582 | 542 global TFT_dive_compass_heading |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
543 TFT_dive_compass_heading: |
| 634 | 544 call compass_heading_common ; compute heading |
| 628 | 545 |
| 623 | 546 ; ; ### for development only, hard-coding the bearing ### |
| 582 | 547 ; ; 244° : SW - W |
| 628 | 548 ; MOVLI .244,xA ; xA used as temp |
| 549 ; MOVII xA,compass_bearing ; compass_bearing is stored in bank isr_backup | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
550 |
| 634 | 551 FONT_SIZE FT_SMALL ; set font size |
| 552 MOVII compass_heading_shown,xA ; get heading | |
| 623 | 553 ; 160° viewing angle: add +360 offset if xA <= 292 for non-negative scale |
| 554 MOVLI .292,sub_a | |
| 555 MOVII xA, sub_b | |
| 628 | 556 call subU16 ; sub_c = sub_a - sub_b |
| 557 btfsc neg_flag ; xA > 292 ? | |
| 558 bra TFT_dive_compass_heading_1 ; YES | |
| 559 ADDLI .360,xA ; NO - add offset | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
560 TFT_dive_compass_heading_1: |
| 628 | 561 SUBLI .80,xA ; subtract 80 (left pixel offset from the center) |
| 562 MOVII xA,xRD ; save result to xRD | |
| 563 ADDLI .160,xA ; add 160 (display with in pixels) | |
| 564 MOVII xA,xRDr ; save result to xRDr | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
565 |
| 628 | 566 btfss compass_bearing_set ; is a bearing set? |
| 567 bra TFT_dive_compass_ruler ; NO - skip next calculations | |
| 568 MOVII xRDr,sub_a ; YES - calculate xRD180 = xRDr - 180 | |
| 623 | 569 MOVLI .180,sub_b |
| 628 | 570 call subU16 ; sub_c = sub_a - sub_b |
| 623 | 571 MOVII sub_c,xRD180 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
572 |
| 258 | 573 TFT_dive_compass_bearing_1: |
| 582 | 574 ; calculate bearing position and visibility (ahead or behind) |
| 628 | 575 bcf compass_bearing_vis ; default is not-visible |
| 576 bcf compass_bearing_ahd ; default is behind | |
| 623 | 577 MOVII compass_bearing,xA |
| 628 | 578 ADDLI .360,xA ; calculate the bearing virtual display offset |
| 579 MOVII xA,divA ; save it for reuse for upper/lower turns and ahead/behind checks | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
580 |
| 623 | 581 ; check if bearing is ahead |
| 628 | 582 MOVII divA,sub_a ; load the bearing offset into sub_a |
| 583 MOVII xRD, sub_b ; load the display offset back to sub_b | |
| 582 | 584 rcall TFT_dive_compass_bearing_ap |
| 628 | 585 btfsc compass_bearing_vis ; bearing visible? |
| 586 bra TFT_dive_compass_bearing_dir; YES | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
587 |
| 628 | 588 ; check if it is ahead with an upper turn |
| 589 MOVII divA,sub_a ; load the bearing offset into sub_a | |
| 590 MOVII xRD, sub_b ; load the display offset back to sub_b | |
| 623 | 591 ADDLI .360,sub_b |
| 582 | 592 rcall TFT_dive_compass_bearing_ap |
| 628 | 593 btfsc compass_bearing_vis ; bearing visible? |
| 594 bra TFT_dive_compass_bearing_dir; YES | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
595 |
| 628 | 596 ; check if it is ahead with a lower turn |
| 597 MOVII divA,sub_a ; load the bearing offset into sub_a | |
| 623 | 598 ADDLI .360,sub_a |
| 628 | 599 MOVII xRD, sub_b ; load the display offset back to sub_b |
| 582 | 600 rcall TFT_dive_compass_bearing_ap |
| 628 | 601 btfsc compass_bearing_vis ; bearing visible? |
| 602 bra TFT_dive_compass_bearing_dir; YES | |
| 258 | 603 |
| 623 | 604 ; marker is not ahead of us, check if it is behind of us |
| 582 | 605 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display |
| 628 | 606 MOVII xRD180,sub_a ; load the display offset back to sub_a |
| 607 MOVII divA, sub_b ; load the marker's offset into sub_b | |
| 582 | 608 rcall TFT_dive_compass_bearing_bp |
| 628 | 609 btfsc compass_bearing_vis ; bearing behind of us? |
| 610 bra TFT_dive_compass_bearing_dir; YES | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
611 |
| 628 | 612 ; check if it is behind with the upper turn |
| 623 | 613 MOVII xRD180,sub_a |
| 614 MOVII divA, sub_b | |
| 615 ADDLI .360, sub_b | |
| 616 rcall TFT_dive_compass_bearing_bp | |
| 628 | 617 btfsc compass_bearing_vis ; bearing behind of us? |
| 618 bra TFT_dive_compass_bearing_dir; YES | |
| 623 | 619 |
| 628 | 620 ; check if it is behind with the lower turn |
| 623 | 621 MOVII xRD180,sub_a |
| 622 ADDLI .360, sub_a | |
| 628 | 623 MOVII divA, sub_b ; load the marker's offset into sub_b |
| 582 | 624 rcall TFT_dive_compass_bearing_bp |
| 625 bra TFT_dive_compass_bearing_dir | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
626 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
627 TFT_dive_compass_bearing_ap: |
| 582 | 628 ; xCM received in sub_a |
| 629 ; xRD received in sub_b | |
| 630 ; 1/a. check if it's viewable from the left side | |
| 628 | 631 call subU16 ; sub_c = sub_a - sub_b |
| 632 btfsc neg_flag ; xRD > divA ? | |
| 633 return ; NO - done | |
| 634 MOVII sub_c,xC ; YES - store the RO=RP-RD for drawing | |
| 623 | 635 ; 1/b. check if it's viewable from the right side |
| 628 | 636 ADDLI .2,sub_a ; avoid thin mess on the side of the display |
| 637 ADDLI .158,sub_b ; load the display offset right side into sub_b | |
| 638 call subU16 ; sub_c = sub_a - sub_b | |
| 639 btfss neg_flag ; xRDr > xA(+2) ? | |
| 640 return ; NO - done | |
| 641 ; YES - print the bearing lines on the screen | |
| 582 | 642 movff xC+0,xCM |
| 628 | 643 bsf compass_bearing_vis ; set visible |
| 644 bsf compass_bearing_ahd ; set ahead | |
| 645 return ; done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
646 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
647 TFT_dive_compass_bearing_bp: |
| 582 | 648 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display |
| 649 ; the marker's offset received in sub_b | |
| 650 ; the xRD180 display offset received in sub_a | |
| 651 ; xRD180 - xCM | |
| 628 | 652 call subU16 ; sub_c = sub_a - sub_b |
| 653 btfsc neg_flag ; CM > xRD180 ? | |
| 654 return ; NO - not on screen, done | |
| 655 ; YES - check 160 - (X) | |
| 656 MOVLI .158, sub_a ; 158 to avoid thin mess on the side of the display | |
| 623 | 657 MOVII sub_c,sub_b |
| 628 | 658 call subU16 ; sub_c = sub_a - sub_b |
| 659 btfsc neg_flag ; X > 160 ? | |
| 660 return ; NO - not on screen, done | |
| 623 | 661 ; check if not overflow - this sounds like a double check... |
| 628 | 662 tstfsz sub_c+1 ; high byte = 0 ? |
| 663 return ; NO - sub_c must be > 160 then, done | |
| 664 movlw d'158' ; YES - load a 158 | |
| 665 cpfslt sub_c+0 ; - low byte < 158 ? | |
| 666 return ; NO - done | |
| 667 movff sub_c+0,xCM ; YES - print the bearing lines on the screen | |
| 668 bsf compass_bearing_vis ; - flag to show bearing lines | |
| 669 return ; - done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
670 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
671 TFT_dive_compass_bearing_dir: |
| 582 | 672 ; check if bearing to heading, and calculate the direction |
| 673 bcf compass_bearing_eq | |
| 674 btfss compass_bearing_vis | |
| 675 bra TFT_dive_compass_bearing_lr | |
| 676 btfss compass_bearing_ahd | |
| 677 bra TFT_dive_compass_bearing_lr | |
| 678 movff xCM,xA+0 | |
| 679 movlw d'80' | |
| 680 cpfseq xA+0 | |
| 681 bra TFT_dive_compass_bearing_lr | |
| 682 bsf compass_bearing_eq | |
| 628 | 683 bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
684 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
685 TFT_dive_compass_bearing_lr: |
| 582 | 686 ; get the bearing virtual display offset |
| 623 | 687 MOVII compass_bearing,xA |
| 688 ; xA = xA > 292 ? xA : xA+360 | |
| 689 MOVLI .292,sub_a | |
| 690 MOVII xA, sub_b | |
| 628 | 691 call subU16 ; sub_c = sub_a - sub_b |
| 692 btfsc neg_flag ; xA > 292 ? | |
| 693 bra TFT_dive_compass_bearing_lr_1; YES | |
| 694 ADDLI .360,xA ; NO - add 360 | |
| 623 | 695 |
| 258 | 696 TFT_dive_compass_bearing_lr_1: |
| 582 | 697 ; 1. calculate whether bearing is to left or to right |
| 628 | 698 bsf compass_bearing_lft ; to the left by default |
| 582 | 699 ; xC: save center value to compare the direction to front value |
| 623 | 700 MOVII xA,xC |
| 582 | 701 ; xB: we need the left side for comparison... left = -180 |
| 623 | 702 MOVII xA, sub_a |
| 703 MOVLI .180,sub_b | |
| 628 | 704 call subU16 ; sub_c = sub_a - sub_b |
| 705 MOVII sub_c,xB ; xB has the left side of the 180° distance center | |
| 623 | 706 ; xA = xRD > (xC+100) ? RD-280 : xRD+80 |
| 707 MOVII xC, sub_a | |
| 708 ADDLI .100,sub_a | |
| 709 MOVII xRD, sub_b | |
| 628 | 710 call subU16 ; sub_c = sub_a - sub_b |
| 711 btfsc neg_flag ; xRD > xC + 100 ? | |
| 712 bra TFT_dive_compass_bearing_lr_2; YES - xA = xRD - 280 | |
| 713 ; NO - xA = xRD + 80 | |
| 623 | 714 MOVII xRD,xA |
| 715 ADDLI .80,xA | |
| 582 | 716 bra TFT_dive_compass_bearing_lr_c |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
717 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
718 TFT_dive_compass_bearing_lr_2: |
| 623 | 719 MOVII xRD,sub_a |
| 720 MOVLI .280,sub_b | |
| 628 | 721 call subU16 ; sub_c = sub_a - sub_b |
| 623 | 722 MOVII sub_c,xA |
| 582 | 723 ;bra TFT_dive_compass_bearing_lr_c |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
724 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
725 TFT_dive_compass_bearing_lr_c: |
| 582 | 726 ; xB < xA < xC => right, otherwise left (default) |
| 623 | 727 MOVII xA,sub_b |
| 728 MOVII xB,sub_a | |
| 628 | 729 call subU16 ; sub_c = sub_a - sub_b |
| 730 btfss neg_flag ; xA > xB ? | |
| 731 bra TFT_dive_compass_ruler ; NO - xB >= xA, keep default left | |
| 623 | 732 MOVII xA,sub_a |
| 733 MOVII xC,sub_b | |
| 628 | 734 call subU16 ; sub_c = sub_a - sub_b |
| 735 btfss neg_flag ; xC > xA ? | |
| 736 bra TFT_dive_compass_ruler ; NO - xA >= xC, keep default left | |
| 582 | 737 bcf compass_bearing_lft |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
738 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
739 TFT_dive_compass_ruler: |
| 582 | 740 ; calculate mod15 for the ticks |
| 623 | 741 MOVII xRD,xA |
| 742 MOVLI .15,xB | |
| 628 | 743 call div16x16 ; xA/xB=xC with xA+0 as remainder |
| 623 | 744 ; check the remainder |
| 582 | 745 movlw d'0' |
| 628 | 746 cpfsgt xA+0 ; mod15 > 0 ? |
| 747 bra TFT_dive_compass_ruler_1 ; NO - RM = 0 | |
| 748 ; YES - RM = 15 - RDmod15 | |
| 582 | 749 movlw d'15' |
| 623 | 750 subfwb xA+0,F |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
751 TFT_dive_compass_ruler_1: |
| 628 | 752 movff xA+0,lo ; xA+0 holds the RM, store it to 'lo' |
| 753 clrf hi ; initialize DD to zero, store it to 'hi' | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
754 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
755 TFT_dive_compass_ruler_loop: |
| 623 | 756 ; 1. check if we run out of the display |
| 628 | 757 movlw d'159' ; looks like 159 works because TFT_box limits the display |
| 582 | 758 cpfslt lo,1 |
| 628 | 759 bra TFT_dive_compass_ruler_lend ; xRM >= W |
| 582 | 760 ; 2. Clear the tick area from DD to RM - in segments to avoid blinking |
| 761 ; don't do a clear if we are at 0 (zero) otherwise it will blink | |
| 762 ; because of the width underflow | |
| 763 movlw d'0' | |
| 764 cpfsgt lo,1 | |
| 765 bra TFT_dive_compass_ruler_loop_zz | |
| 766 rcall TFT_dive_compass_clr_ruler | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
767 TFT_dive_compass_ruler_loop_zz: |
| 582 | 768 ; 3. Draw the markers @ RM |
| 769 ; we receive RM in lo and DD in hi | |
| 770 movlw d'2' | |
| 628 | 771 movwf win_bargraph ; set with of ticks |
| 582 | 772 movwf win_width+0 |
| 773 clrf win_width+1 | |
| 628 | 774 movff lo,win_leftx2 ; 0..159 |
| 775 movlw dm_custom_compass_tick_top_top | |
| 776 movwf win_top ; set position for upper ticks | |
| 582 | 777 movlw dm_custom_compass_tick_height |
| 628 | 778 movwf win_height ; set hight of ticks |
| 634 | 779 movf pallet_color_memo,W ; select color |
| 780 BOX_COLOR ; draw tick | |
| 628 | 781 movlw dm_custom_compass_tick_bot_top |
| 782 movwf win_top ; set position for lower ticks | |
| 783 movlw dm_custom_compass_tick_height | |
| 784 movwf win_height ; set hight of ticks | |
| 634 | 785 movf pallet_color_memo,W ; select color |
| 786 BOX_COLOR ; draw tick | |
| 628 | 787 ; 4. If D < 82 and RM > 79: means we put something over the center line, |
| 788 ; so redraw the center line | |
| 582 | 789 movlw d'82' |
| 790 cpfslt hi,1 | |
| 791 bra TFT_dive_compass_ruler_loop_zz2 | |
| 792 movlw d'79' | |
| 793 cpfsgt lo,1 | |
| 794 bra TFT_dive_compass_ruler_loop_zz2 | |
| 623 | 795 ; enough to print center line as bearing marker is not in the ticker area |
| 628 | 796 rcall TFT_dive_compass_c ; draw center line in yellow |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
797 TFT_dive_compass_ruler_loop_zz2: |
| 582 | 798 ; 5. set D = RM + 2 : position after the 2px tick |
| 799 movff lo,hi | |
| 800 movlw d'2' | |
| 801 addwf hi,F | |
| 802 ; 6. set RM = RM + 15 : position to the next tick | |
| 803 movlw d'15' | |
| 804 addwf lo,F | |
| 805 ; 7. loop | |
| 806 bra TFT_dive_compass_ruler_loop | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
807 |
| 628 | 808 TFT_dive_compass_ruler_lend: ; loop end |
| 809 ; 8. clear the rest of the tick area if D < 160 | |
| 582 | 810 movlw d'160' |
| 811 cpfslt hi | |
| 628 | 812 bra TFT_dive_compass_labels ; D >= W |
| 582 | 813 ; 9. position left to end of display to clear the remaining area |
| 814 movlw d'159' | |
| 815 movwf lo | |
| 816 ; 10. clear it | |
| 817 rcall TFT_dive_compass_clr_ruler | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
818 |
| 628 | 819 TFT_dive_compass_labels: |
| 582 | 820 ; done with the compass ruler, put the labels on the screen |
| 634 | 821 FONT_COLOR_MEMO ; set dive-mode standard color |
| 628 | 822 clrf hi ; hi stores the display position |
| 823 movff hi,xHI ; bank-safe clear of xHI | |
| 824 clrf lo ; lo stores the last item's display position | |
| 825 movff lo,xLO ; bank-safe clear of xLO | |
| 826 MOVLI .219,sub_a ; position of the cardinal | |
| 827 MOVII xRD, sub_b ; get the RD back to sub_b | |
| 828 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 829 btfss compass_show_cardinal ; shall show cardinal? | |
| 830 bra dcr_1 ; NO | |
| 831 STRCPY_TEXT_PRINT tSW ; YES - print it | |
| 582 | 832 dcr_1: |
| 628 | 833 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 834 MOVLI .267,sub_a ; position of the cardinal | |
| 835 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 836 btfss compass_show_cardinal ; shall show cardinal? | |
| 837 bra dcr_2 ; NO | |
| 838 STRCPY_TEXT_PRINT tW ; YES - print it | |
| 582 | 839 dcr_2: |
| 628 | 840 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 841 MOVLI .309,sub_a ; position of the cardinal | |
| 842 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 843 btfss compass_show_cardinal ; shall show cardinal? | |
| 844 bra dcr_3 ; NO | |
| 845 STRCPY_TEXT_PRINT tNW ; YES - print it | |
| 582 | 846 dcr_3: |
| 628 | 847 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 848 MOVLI .358,sub_a ; position of the cardinal | |
| 849 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 850 btfss compass_show_cardinal ; shall show cardinal? | |
| 851 bra dcr_4 ; NO | |
| 852 STRCPY_TEXT_PRINT tN ; YES - print it | |
| 582 | 853 dcr_4: |
| 628 | 854 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 855 MOVLI .399,sub_a ; position of the cardinal | |
| 856 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 857 btfss compass_show_cardinal ; shall show cardinal? | |
| 858 bra dcr_5 ; NO | |
| 859 STRCPY_TEXT_PRINT tNE ; YES - print it | |
| 582 | 860 dcr_5: |
| 628 | 861 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 862 MOVLI .448,sub_a ; position of the cardinal | |
| 863 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 864 btfss compass_show_cardinal ; shall show cardinal? | |
| 865 bra dcr_6 ; NO | |
| 866 STRCPY_TEXT_PRINT tE ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
867 dcr_6: |
| 628 | 868 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 869 MOVLI .489,sub_a ; position of the cardinal | |
| 870 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 871 btfss compass_show_cardinal ; shall show cardinal? | |
| 872 bra dcr_7 ; NO | |
| 873 STRCPY_TEXT_PRINT tSE ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
874 dcr_7: |
| 628 | 875 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 876 MOVLI .538,sub_a ; position of the cardinal | |
| 877 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 878 btfss compass_show_cardinal ; shall show cardinal? | |
| 879 bra dcr_8 ; NO | |
| 880 STRCPY_TEXT_PRINT tS ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
881 dcr_8: |
| 628 | 882 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 883 MOVLI .579,sub_a ; position of the cardinal | |
| 884 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 885 btfss compass_show_cardinal ; shall show cardinal? | |
| 886 bra dcr_9 ; NO | |
| 887 STRCPY_TEXT_PRINT tSW ; YES - print it | |
| 582 | 888 dcr_9: |
| 628 | 889 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 890 MOVLI .627,sub_a ; position of the cardinal | |
| 891 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 892 btfss compass_show_cardinal ; shall show cardinal? | |
| 893 bra dcr_10 ; NO | |
| 894 STRCPY_TEXT_PRINT tW ; YES - print it | |
| 582 | 895 dcr_10: |
| 628 | 896 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 897 MOVLI .669,sub_a ; position of the cardinal | |
| 898 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 899 btfss compass_show_cardinal ; shall show cardinal? | |
| 900 bra dcr_11 ; NO | |
| 901 STRCPY_TEXT_PRINT tNW ; YES - print it | |
| 582 | 902 dcr_11: |
| 628 | 903 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 904 MOVLI .718,sub_a ; position of the cardinal | |
| 905 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 906 btfss compass_show_cardinal ; shall show? | |
| 907 bra dcr_12 ; NO | |
| 908 STRCPY_TEXT_PRINT tN ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
909 dcr_12: |
| 628 | 910 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
911 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
912 TFT_dive_compass_label_end: |
| 628 | 913 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 582 | 914 ; restore lo and hi for the final cleanup |
| 628 | 915 movff xLO,lo ; xLO and xHI are stored in bank isr_backup |
| 582 | 916 movff xHI,hi |
| 628 | 917 ; clear the rest of the SQ area if there is more space |
| 582 | 918 movlw d'159' |
| 919 cpfslt hi | |
| 628 | 920 bra TFT_dive_compass_label_end2 ; D >= 160, no more space |
| 582 | 921 ; position left to end of display to clear the remaining area |
| 922 movlw d'158' | |
| 923 movwf lo | |
| 924 ; clear it | |
| 925 rcall TFT_dive_compass_clr_label | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
926 TFT_dive_compass_label_end2: |
| 628 | 927 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 582 | 928 ; do we have bearing set? |
| 628 | 929 btfsc compass_bearing_set ; bearing set? |
| 930 bra TFT_dive_compass_dir_text ; YES - print the direction (<< or >>) | |
| 931 rcall TFT_dive_compass_dir_lclr ; NO - clear the area (e.g. we had but removed) | |
| 582 | 932 rcall TFT_dive_compass_dir_rclr |
| 933 bra TFT_dive_compass_text | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
934 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
935 TFT_dive_compass_dir_text: |
| 582 | 936 ; bearing set, but does it point to heading? |
| 937 btfss compass_bearing_eq | |
| 628 | 938 bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the direction |
| 939 rcall TFT_dive_compass_dir_lclr ; bearing == heading - no need for direction markers | |
| 582 | 940 rcall TFT_dive_compass_dir_rclr |
| 941 bra TFT_dive_compass_text | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
942 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
943 TFT_dive_compass_dir_text_2: |
| 634 | 944 FONT_COLOR color_green |
| 582 | 945 btfsc compass_bearing_lft |
| 628 | 946 bra TFT_dive_compass_dir_ldir ; bearing_lft=1, print the left marker |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
947 ;TFT_dive_compass_text_rdir: |
| 582 | 948 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row-.2 |
| 949 STRCPY_PRINT ">>" | |
| 628 | 950 rcall TFT_dive_compass_dir_lclr ; do not forget to clear the left |
| 582 | 951 bra TFT_dive_compass_text |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
952 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
953 TFT_dive_compass_dir_ldir: |
| 582 | 954 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2 |
| 955 STRCPY_PRINT "<<" | |
| 628 | 956 rcall TFT_dive_compass_dir_rclr ; do not forget to clear the right |
| 582 | 957 ;bra TFT_dive_compass_text |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
958 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
959 TFT_dive_compass_text: |
| 582 | 960 ; Clear some unused space on the right mH |
| 961 WIN_BOX_BLACK dm_custom_compass_tick_top_bot+.1,dm_custom_compass_tick_bot_top-.1,.158,.159 ; top, bottom, left, right | |
| 371 | 962 |
| 582 | 963 ; Text output |
| 964 WIN_SMALL dm_custom_compass_head_column, dm_custom_compass_head_row | |
| 634 | 965 call TFT_surface_compass_heading_com ; show "xxx° N" |
| 582 | 966 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
967 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
968 TFT_dive_compass_dir_lclr: |
| 582 | 969 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2 |
| 970 STRCPY_PRINT " " | |
| 971 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
972 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
973 TFT_dive_compass_dir_rclr: |
| 582 | 974 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row-.2 |
| 975 STRCPY_PRINT " " | |
| 976 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
977 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
978 TFT_dive_compass_label_proc: |
| 582 | 979 movlw d'14' |
| 628 | 980 movwf up ; cardinal width in px |
| 623 | 981 bcf compass_show_cardinal |
| 582 | 982 ; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ? |
| 983 ; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP): | |
| 623 | 984 MOVII xRD,sub_b |
| 628 | 985 call subU16 ; sub_c = sub_a - sub_b |
| 986 btfsc neg_flag ; >= 0 ? | |
| 987 return ; NO | |
| 582 | 988 ; store the RO=RP-RD for drawing |
| 623 | 989 MOVII sub_c,xC |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
990 |
| 582 | 991 ; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160 |
| 992 ; if already above, no need to process the rest of the labels | |
| 628 | 993 movff up,WREG ; take care about the width |
| 582 | 994 addwf sub_a+0,1 |
| 995 btfsc STATUS, C | |
| 996 incf sub_a+1 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
997 |
| 623 | 998 MOVII xRDr,sub_b |
| 628 | 999 call subU16 ; sub_c = sub_a - sub_b |
| 1000 btfss neg_flag ; < 0 ? | |
| 1001 bra TFT_dive_compass_label_end ; NO | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1002 |
| 582 | 1003 ; 2. restore RO=RP-RD from 1/a. |
| 1004 movff xC+0,lo | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1005 |
| 582 | 1006 ; 3. Clear the segment from DD(hi) to lo |
| 1007 ; don't do a clear if we are at 0 (zero) otherwise it will blink | |
| 1008 ; ?because of the width underflow? | |
| 1009 movlw d'1' | |
| 1010 cpfsgt lo | |
| 1011 bra TFT_dive_compass_label_proc_p | |
| 1012 rcall TFT_dive_compass_clr_label | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1013 TFT_dive_compass_label_proc_p: |
| 582 | 1014 ; 4. print the SQ on the screen |
| 634 | 1015 FONT_COLOR_MEMO |
| 623 | 1016 bsf compass_show_cardinal |
|
257
5dd0f39d05d4
minor speed and size improvements for the compass routine
heinrichsweikamp
parents:
256
diff
changeset
|
1017 ;TFT_dive_compass_label_print: |
| 634 | 1018 movlw dm_custom_compass_label_row ; set output position |
| 1019 movff WREG,win_top ; ... | |
| 1020 movff lo,win_leftx2 ; ... | |
| 1021 FONT_SIZE FT_SMALL ; set font size | |
| 582 | 1022 ; 6. retain the new display positions |
| 1023 movff lo,hi | |
| 1024 movff up,WREG | |
| 1025 addwf hi,F | |
| 1026 movff lo,xLO | |
| 1027 movff hi,xHI | |
| 1028 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1029 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1030 TFT_dive_compass_c_mk: |
| 582 | 1031 ; Common task to draw center line and marker |
| 1032 ; until a proper implementation make it simple: | |
| 1033 rcall TFT_dive_compass_mk | |
| 628 | 1034 TFT_dive_compass_c: |
| 582 | 1035 movlw color_yellow |
| 1036 WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81 ; center line in yellow | |
| 1037 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1038 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1039 TFT_dive_compass_mk: |
| 582 | 1040 ; draw the bearing on the screen if visible and if we just put something over it |
| 628 | 1041 btfss compass_bearing_set ; bearing set? |
| 1042 return ; NO - done | |
| 1043 btfss compass_bearing_vis ; YES - bearing visible? | |
| 1044 return ; NO - bearing set but not visible, done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1045 |
| 582 | 1046 ; save lo/hi from trashing |
| 623 | 1047 MOVII mpr,xA |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1048 |
| 582 | 1049 ; did we just update the marker's position? |
| 1050 ; DD.......DD | |
| 1051 ; CM+2>=DD(old) or CM-2<=DD | |
| 1052 ; ToDo | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1053 |
| 582 | 1054 btfss compass_bearing_ahd |
| 1055 bra TFT_dive_compass_mk_rear | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1056 ;TFT_dive_compass_mk_front: |
| 582 | 1057 clrf lo |
| 1058 movff xCM,lo | |
| 628 | 1059 bsf compass_show_cardinal ; set=green marker |
| 582 | 1060 rcall TFT_dive_compass_mk_print |
| 623 | 1061 bcf compass_show_cardinal |
| 582 | 1062 bra TFT_dive_compass_mk_end |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1063 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1064 TFT_dive_compass_mk_rear: |
| 582 | 1065 clrf lo |
| 1066 movff xCM,lo | |
| 628 | 1067 bcf compass_show_cardinal ; set=red marker |
| 582 | 1068 rcall TFT_dive_compass_mk_print |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1069 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1070 TFT_dive_compass_mk_end: |
| 623 | 1071 MOVII xA,mpr |
| 582 | 1072 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1073 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1074 TFT_dive_compass_mk_print: |
| 582 | 1075 movlw d'1' |
| 1076 cpfsgt lo | |
| 628 | 1077 bra TFT_dive_compass_mk_print_2 ; lo <= 1, skip the first line |
| 582 | 1078 movlw d'2' |
| 1079 subwf lo,0 | |
| 1080 ; movff WREG,win_leftx2 | |
| 1081 rcall TFT_dive_compass_mk_print_3 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1082 TFT_dive_compass_mk_print_2: |
| 582 | 1083 ; save hi/lo |
| 623 | 1084 MOVII mpr,divA |
| 582 | 1085 ; clear the middle of the bearing marker |
| 1086 movff lo,hi | |
| 1087 movlw d'2' | |
| 1088 addwf lo,1 | |
| 1089 rcall TFT_dive_compass_clr_label | |
| 1090 ; restore hi/lo | |
| 623 | 1091 MOVII divA,mpr |
| 582 | 1092 ; print a dot on the middle |
| 623 | 1093 movf lo,W |
| 582 | 1094 rcall TFT_dive_compass_mk_print_dot |
| 1095 ; finally print the right marker line | |
| 1096 movlw d'2' | |
| 1097 addwf lo,0 | |
| 1098 ; rcall TFT_dive_compass_mk_print_3 | |
| 1099 ; return | |
|
257
5dd0f39d05d4
minor speed and size improvements for the compass routine
heinrichsweikamp
parents:
256
diff
changeset
|
1100 TFT_dive_compass_mk_print_3: |
| 582 | 1101 movwf win_leftx2 |
| 1102 movlw dm_custom_compass_label_row | |
| 1103 movwf win_top | |
| 1104 movlw dm_custom_compass_label_height-.2 | |
| 1105 movwf win_height | |
| 1106 bra TFT_dive_compass_mk_print_4 | |
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
parents:
259
diff
changeset
|
1107 TFT_dive_compass_mk_print_dot: |
| 582 | 1108 movwf win_leftx2 |
| 1109 movlw dm_custom_compass_label_row + .9 | |
| 1110 movwf win_top | |
| 1111 movlw d'4' | |
| 1112 movwf win_height | |
|
264
9dbdb060d44c
Fix compass ruler's missing bottom ticks; Clear between the bearing marker lines and draw a dot.
janos_kovacs <kovjanos@gmail.com>
parents:
259
diff
changeset
|
1113 TFT_dive_compass_mk_print_4: |
| 582 | 1114 movlw .158 |
| 1115 cpfslt win_leftx2 | |
| 1116 bra TFT_dive_compass_mk_print_5 | |
| 1117 movlw d'2' | |
| 623 | 1118 movwf win_bargraph |
| 582 | 1119 movwf win_width+0 |
| 1120 clrf win_width+1 | |
| 1121 movlw color_green | |
| 623 | 1122 btfss compass_show_cardinal |
| 634 | 1123 movlw color_red ; select color |
| 1124 BOX_COLOR ; draw box | |
| 371 | 1125 TFT_dive_compass_mk_print_5: |
| 582 | 1126 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1127 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1128 TFT_dive_compass_clr_label: |
| 582 | 1129 movlw dm_custom_compass_label_row-.2 ; set top & height |
| 623 | 1130 movwf win_top |
| 582 | 1131 movlw dm_custom_compass_label_height+.2 |
| 623 | 1132 movwf win_height |
| 582 | 1133 rcall TFT_dive_compass_clear |
| 1134 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1135 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1136 TFT_dive_compass_clr_ruler: |
| 582 | 1137 ; top tick |
| 1138 movlw dm_custom_compass_tick_top_top ; set top & height | |
| 623 | 1139 movwf win_top |
| 582 | 1140 movlw dm_custom_compass_tick_height |
| 623 | 1141 movwf win_height |
| 582 | 1142 rcall TFT_dive_compass_clear |
| 1143 ;bottom tick | |
| 1144 movlw dm_custom_compass_tick_bot_top ; set top & height | |
| 623 | 1145 movwf win_top |
| 582 | 1146 movlw dm_custom_compass_tick_height |
| 623 | 1147 movwf win_height |
| 582 | 1148 ; rcall TFT_dive_compass_clear |
| 1149 ; return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1150 TFT_dive_compass_clear: |
| 582 | 1151 ; we receive RM in lo and DD in hi |
| 1152 ; calculate width = RM-D | |
| 623 | 1153 movf hi,W |
| 582 | 1154 subwf lo,W |
| 623 | 1155 bz TFT_dive_compass_clear3 ; do nothing if there is nothing to do |
| 582 | 1156 movwf win_width+0 ; RM-DD |
| 1157 movwf win_bargraph | |
| 1158 clrf win_width+1 | |
| 1159 movlw .1 | |
| 1160 cpfsgt win_width+0 | |
| 623 | 1161 bra TFT_dive_compass_clear3 ; do not clear a single pixel (or less) |
| 582 | 1162 movff hi,win_leftx2 |
| 634 | 1163 movlw color_black ; select color |
| 1164 BOX_COLOR ; draw box | |
| 370 | 1165 TFT_dive_compass_clear3: |
| 582 | 1166 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1167 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1168 tft_compass_cardinal: |
| 623 | 1169 btfsc hi,0 ; heading > 255° ? |
| 1170 bra tft_compass_cardinal2 ; YES - must be W, NW or N | |
| 1171 ; NO - must be W, SW, S, SE, E, NE or N | |
| 582 | 1172 movlw .23 |
| 1173 subwf lo,W | |
| 1174 btfss STATUS,C | |
| 1175 bra tft_compass_cardinal_N | |
| 1176 movlw .68 | |
| 1177 subwf lo,W | |
| 1178 btfss STATUS,C | |
| 1179 bra tft_compass_cardinal_NE | |
| 1180 movlw .113 | |
| 1181 subwf lo,W | |
| 1182 btfss STATUS,C | |
| 1183 bra tft_compass_cardinal_E | |
| 1184 movlw .158 | |
| 1185 subwf lo,W | |
| 1186 btfss STATUS,C | |
| 1187 bra tft_compass_cardinal_SE | |
| 1188 movlw .203 | |
| 1189 subwf lo,W | |
| 1190 btfss STATUS,C | |
| 1191 bra tft_compass_cardinal_S | |
| 1192 movlw .248 | |
| 1193 subwf lo,W | |
| 1194 btfss STATUS,C | |
| 1195 bra tft_compass_cardinal_SW | |
| 1196 bra tft_compass_cardinal_W | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1197 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1198 tft_compass_cardinal2: |
| 582 | 1199 movlw .37 |
| 1200 subwf lo,W | |
| 1201 btfss STATUS,C | |
| 1202 bra tft_compass_cardinal_W | |
| 1203 movlw .82 | |
| 1204 subwf lo,W | |
| 1205 btfss STATUS,C | |
| 1206 bra tft_compass_cardinal_NW | |
| 1207 ; bra tft_compass_cardinal_N | |
| 1208 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1209 tft_compass_cardinal_N: |
| 582 | 1210 STRCAT_TEXT tN |
| 1211 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1212 tft_compass_cardinal_NE: |
| 582 | 1213 STRCAT_TEXT tNE |
| 1214 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1215 tft_compass_cardinal_E: |
| 582 | 1216 STRCAT_TEXT tE |
| 1217 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1218 tft_compass_cardinal_SE: |
| 582 | 1219 STRCAT_TEXT tSE |
| 1220 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1221 tft_compass_cardinal_S: |
| 582 | 1222 STRCAT_TEXT tS |
| 1223 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1224 tft_compass_cardinal_SW: |
| 582 | 1225 STRCAT_TEXT tSW |
| 1226 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1227 tft_compass_cardinal_W: |
| 582 | 1228 STRCAT_TEXT tW |
| 1229 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1230 tft_compass_cardinal_NW: |
| 582 | 1231 STRCAT_TEXT tNW |
| 1232 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1233 |
| 634 | 1234 ;============================================================================= |
| 1235 compass_ops6 CODE | |
| 1236 ;============================================================================= | |
| 1237 | |
| 628 | 1238 ;----------------------------------------------------------------------------- |
| 634 | 1239 ; get and process heading |
| 1240 ; | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1241 compass_heading_common: |
| 623 | 1242 btfss compass_enabled ; compass enabled? |
| 628 | 1243 bra compass_heading_common_zero ; NO |
| 623 | 1244 |
| 628 | 1245 ; Get an averaged new heading |
| 623 | 1246 movlw compass_averaging ; number of averaging cycles |
| 1247 movwf up ; initialize loop counter | |
| 628 | 1248 compass_heading_common_1: |
| 623 | 1249 call I2C_RX_compass ; test compass |
| 1250 call I2C_RX_accelerometer ; test accelerometer | |
| 1251 call compass_filter ; filter raw compass + accelerometer readings | |
| 1252 decfsz up,F ; decrement loop counter, done? | |
| 628 | 1253 bra compass_heading_common_1 ; NO - loop |
| 623 | 1254 |
| 628 | 1255 call compass ; do compass correction (C-code) |
| 623 | 1256 banksel common ; back to bank common |
| 1257 | |
| 628 | 1258 ; Check for calibration and change |
| 1259 MOVII compass_heading_shown,sub_a ; transfer shown heading to sub_a | |
| 1260 MOVII compass_heading_new, sub_b ; transfer new heading to sub_b | |
| 623 | 1261 |
| 1262 btfsc sub_b+1,7 ; valid compass calibration? | |
| 628 | 1263 bra compass_heading_common_zero ; NO |
| 1264 | |
| 1265 movf sub_a+0,W ; get shown heading, low byte | |
| 1266 cpfseq sub_b+0 ; compare with new heading, low byte, equal? | |
| 1267 bra compass_heading_common_2 ; NO - not equal | |
| 1268 movf sub_a+1,W ; get shown heading, high byte | |
| 1269 cpfseq sub_b+1 ; compare with new heading, high byte, equal? | |
| 1270 bra compass_heading_common_2 ; NO - not equal | |
| 1271 return ; YES - done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1272 |
| 628 | 1273 compass_heading_common_2: |
| 1274 ; turn both headings such that compass_heading_new points to 0° | |
| 1275 call subU16 ; sub_c = compass_heading_shown - compass_heading_new | |
| 1276 btfss neg_flag ; was compass_heading_new in 1st halve? | |
| 1277 bra compass_heading_common_3 ; YES - check where compass_heading_shown is now | |
| 1278 MOVLI .360, sub_a ; NO - overturned, need to turn back to match 360° | |
| 1279 MOVII sub_c,sub_b ; - move overturned compass_heading_new to sub_b | |
| 1280 call subU16 ; - sub_c = angle between overturned compass_heading_shown and 360° | |
| 1281 compass_heading_common_3: | |
| 1282 ; check if turned compass_heading_shown is in 1st or 2nd halve | |
| 1283 MOVII sub_c,sub_a ; sub_a = turned compass_heading_shown | |
| 1284 MOVLI .180, sub_b ; sub_b = begin of 2nd halve | |
| 1285 call cmpU16 ; check (turned compass_heading_shown) - 180° | |
| 1286 btfss neg_flag ; result negative? | |
| 1287 bra compass_heading_common_5 ; NO - in 2nd halve, increment towards compass_heading_new | |
| 1288 ;bra compass_heading_common_4 ; YES - in 1st halve, decrement towards compass_heading_new | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1289 |
| 628 | 1290 compass_heading_common_4: |
| 1291 ; decrement compass_heading_shown towards compass_heading_new | |
| 1292 rcall compass_heading_stepsize_2 ; calculate step size and put it into sub_b | |
| 1293 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a | |
| 1294 call subU16 ; decrement heading: sub_c = compass_heading_shown - step size | |
| 1295 btfss neg_flag ; did an under-run occur? | |
| 1296 bra compass_heading_common_6 ; NO - store result | |
| 1297 MOVLI .360, sub_a ; YES - wrap around 360° | |
| 1298 MOVII sub_c,sub_b ; - transfer decrement result to sub_b | |
| 1299 call subU16 ; - wrap decrement result around | |
| 1300 bra compass_heading_common_6 ; - store wrapped result | |
| 1301 | |
| 1302 compass_heading_common_5: | |
| 1303 ; increment compass_heading_shown towards compass_heading_new | |
| 1304 rcall compass_heading_stepsize_1 ; calculate step size and put it into sub_b | |
| 1305 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a | |
| 1306 call addU16 ; increment heading: sub_c = compass_heading_shown + step size | |
| 1307 MOVII sub_c,sub_a ; transfer increment result to sub_a | |
| 1308 MOVLI .360, sub_b ; load wrap-around threshold | |
| 1309 call subU16 ; calculate if over-run occurred | |
| 1310 btfss neg_flag ; did an over-run occur? | |
| 1311 bra compass_heading_common_6 ; YES - store already wrapped-around result | |
| 1312 MOVII sub_a,sub_c ; NO - retrieve former straight increment result | |
| 1313 bra compass_heading_common_6 ; - store wrapped result | |
| 1314 | |
| 1315 compass_heading_common_zero: | |
| 1316 CLRI sub_c ; set heading to 0° | |
| 1317 ;bra compass_heading_common_6 ; store heading | |
| 1318 | |
| 1319 compass_heading_common_6: | |
| 1320 MOVII sub_c,compass_heading_shown ; store new shown heading | |
| 1321 return ; done | |
| 1322 | |
| 1323 compass_heading_stepsize_1: | |
| 1324 ; turn heading difference (180...359) into a step size | |
| 1325 MOVLI .360, sub_a ; load 360° | |
| 1326 MOVII sub_c,sub_b ; load difference | |
| 1327 call subU16 ; sub_c = 360 - difference (i.e. 1...180 now) | |
| 1328 compass_heading_stepsize_2: | |
| 1329 ; turn heading difference (1...180) into a step size | |
| 1330 bcf STATUS,C ; clear carry | |
| 1331 rrcf sub_c+0 ; heading difference /= 2 | |
| 1332 bcf STATUS,C ; clear carry | |
| 1333 rrcf sub_c+0 ; heading difference /= 2, total /= 4 now | |
| 1334 incf sub_c+0,f ; final += 1 to have one increment / decrement at least | |
| 1335 MOVII sub_c,sub_b ; transfer result to sub_b | |
| 1336 return | |
| 1337 | |
| 623 | 1338 ENDIF ; _compass |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1339 |
| 634 | 1340 ;----------------------------------------------------------------------------- |
| 1341 | |
| 582 | 1342 END |