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