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