Mercurial > public > hwos_code
annotate src/compass_ops.asm @ 628:cd58f7fc86db
3.05 stable work
| author | heinrichsweikamp |
|---|---|
| date | Thu, 19 Sep 2019 12:01:29 +0200 |
| parents | c40025d8e750 |
| children | 237931377539 |
| rev | line source |
|---|---|
| 582 | 1 ;============================================================================= |
| 2 ; | |
| 628 | 3 ; File compass_ops.asm combined next generation V3.03.5 |
| 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 | |
| 628 | 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: | |
| 628 | 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 |
| 628 | 217 ;----------------------------------------------------------------------------- |
| 623 | 218 |
| 582 | 219 global compass_calibration_loop |
| 628 | 220 compass_calibration_loop: ; compass calibration |
| 221 bsf block_sensor_interrupt ; disable sensor interrupts | |
| 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 | |
| 628 | 228 btfss switch_right2 ; wait until button is released |
| 623 | 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 |
| 628 | 237 call request_speed_fastest ; request CPU speed change to fastest speed |
| 623 | 238 |
| 628 | 239 movlw .7 ; initialize gain |
| 582 | 240 movff WREG,opt_compass_gain |
| 428 | 241 |
| 628 | 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 btfsc STATUS,N ; < 0 ? | |
| 252 bra compass_calibration_loop1 ; YES - skip gain stuff (Would hang here in case of compass failure) | |
| 253 banksel common ; bank to bank common | |
| 623 | 254 |
| 582 | 255 call I2C_init_compass |
| 256 | |
| 628 | 257 btfsc compass_type3 ; compass type 3 ? |
| 258 bra compass_calibration_loop1 ; YES - skip gain stuff | |
| 259 btfsc compass_type2 ; compass type 2 ? | |
| 260 bra compass_calibration_loop1 ; YES - skip gain stuff | |
| 582 | 261 |
| 628 | 262 rcall TFT_compass_show_gain ; show the current compass gain |
| 147 | 263 |
| 582 | 264 WAITMS d'250' |
| 628 | 265 WAITMS d'250' ; wait for first reading... |
| 147 | 266 |
| 628 | 267 movlw .60 ; calibration shall run for 60 seconds |
| 268 call reset_timeout_time ; set timeout | |
| 147 | 269 |
| 628 | 270 call I2C_RX_compass ; read compass |
| 271 call I2C_RX_accelerometer ; read accelerometer | |
| 147 | 272 |
| 582 | 273 ; Test all axes for +4096 (Hi byte=16) |
| 628 | 274 banksel compass_DX ; select bank common2 |
| 582 | 275 movlw .16 |
| 276 cpfseq compass_DX+1 | |
| 277 bra $+4 | |
| 278 bra compass_calibration_gainset | |
| 279 cpfseq compass_DY+1 | |
| 280 bra $+4 | |
| 281 bra compass_calibration_gainset | |
| 282 cpfseq compass_DZ+1 | |
| 283 bra $+4 | |
| 284 bra compass_calibration_gainset | |
| 147 | 285 |
| 582 | 286 ; Test all axes for -4096 (Hi byte=240) |
| 287 movlw .240 | |
| 288 cpfseq compass_DX+1 | |
| 289 bra $+4 | |
| 290 bra compass_calibration_gainset | |
| 291 cpfseq compass_DY+1 | |
| 292 bra $+4 | |
| 293 bra compass_calibration_gainset | |
| 294 cpfseq compass_DZ+1 | |
| 295 bra $+4 | |
| 296 bra compass_calibration_gainset | |
| 147 | 297 |
| 628 | 298 compass_calibration_loop1: ; done with gain |
| 299 banksel common ; bank to bank common | |
| 300 rcall compass_filter_init ; set DX_f values | |
| 301 call compass_reset_calibration ; reset CX_f values (C-code) | |
| 302 banksel common ; back to bank common | |
| 147 | 303 |
| 304 compass_calibration_loop2: | |
| 628 | 305 call I2C_RX_compass ; read compass |
| 306 call I2C_RX_accelerometer ; test accelerometer | |
| 307 rcall compass_filter ; filter compass raw data | |
| 147 | 308 |
| 582 | 309 ; Twice |
| 628 | 310 call I2C_RX_compass ; read compass |
| 311 call I2C_RX_accelerometer ; test accelerometer | |
| 312 rcall compass_filter ; filter compass raw data | |
| 582 | 313 |
| 628 | 314 ; btfsc compass_type1 ; compass1? |
| 315 ; bra compass_calibration_loop3 ; YES - skip gain stuff | |
| 147 | 316 |
| 582 | 317 ; Test all axes for +4096 (Hi byte=16) |
| 628 | 318 banksel compass_DX ; select bank common2 |
| 582 | 319 movlw .16 |
| 320 cpfseq compass_DX+1 | |
| 321 bra $+4 | |
| 322 bra compass_calibration_gainset | |
| 323 cpfseq compass_DY+1 | |
| 324 bra $+4 | |
| 325 bra compass_calibration_gainset | |
| 326 cpfseq compass_DZ+1 | |
| 327 bra $+4 | |
| 328 bra compass_calibration_gainset | |
| 147 | 329 |
| 582 | 330 ; Test all axes for -4096 (Hi byte=240) |
| 331 movlw .240 | |
| 332 cpfseq compass_DX+1 | |
| 333 bra $+4 | |
| 334 bra compass_calibration_gainset | |
| 335 cpfseq compass_DY+1 | |
| 336 bra $+4 | |
| 337 bra compass_calibration_gainset | |
| 338 cpfseq compass_DZ+1 | |
| 339 bra $+4 | |
| 340 bra compass_calibration_gainset | |
| 628 | 341 banksel common ; back to bank common |
| 147 | 342 ; |
| 582 | 343 ; ; Three |
| 628 | 344 ; call I2C_RX_compass ; read compass |
| 345 ; call I2C_RX_accelerometer ; test accelerometer | |
| 346 ; call compass_filter ; filter compass raw data | |
| 147 | 347 ; |
| 582 | 348 ; ; Four times to get cleaner values |
| 628 | 349 ; call I2C_RX_compass ; read compass |
| 350 ; call I2C_RX_accelerometer ; test accelerometer | |
| 351 ; call compass_filter ; filter compass raw data | |
| 147 | 352 |
| 428 | 353 compass_calibration_loop3: |
| 623 | 354 ; and register only one value out of four: |
| 628 | 355 call compass_add_calibration ; check and store new max/min values (C-code) |
| 356 banksel common ; back to bank common | |
| 147 | 357 |
| 628 | 358 rcall TFT_compass_fast ; show values |
| 359 btfsc trigger_timeout ; timeout (calibration done)? | |
| 360 bra compass_calibration_exit ; YES - done | |
| 361 btfss trigger_full_second ; NO - new second begun? | |
| 362 bra compass_calibration_loop2 ; NO - loop | |
| 363 bcf trigger_full_second ; YES - clear flag | |
| 364 rcall TFT_show_timeout_testmode ; - show remaining time | |
| 365 bra compass_calibration_loop2 ; - loop | |
| 147 | 366 |
| 367 compass_calibration_exit: | |
| 628 | 368 bcf block_sensor_interrupt ; re-enable sensor interrupts |
| 623 | 369 |
| 628 | 370 call compass_solve_calibration ; calculate calibration factors (C-code) |
| 371 banksel common ; back to bank common | |
| 372 | |
| 373 call request_speed_normal ; request CPU speed change to normal speed | |
| 623 | 374 |
| 628 | 375 call option_save_all ; save all settings into EEPROM |
| 582 | 376 movlw .6 |
| 628 | 377 movff WREG,customview_surfmode ; set to compass view... |
| 378 goto surfloop ; ...and exit | |
| 623 | 379 |
| 628 | 380 ;----------------------------------------------------------------------------- |
| 147 | 381 |
| 582 | 382 global TFT_compass_fast |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
383 TFT_compass_fast: |
| 628 | 384 WIN_TINY .0,.50 |
| 385 STRCPY "Cx:" | |
| 623 | 386 MOVII compass_DX,mpr |
| 628 | 387 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required |
| 388 output_16 | |
| 389 STRCAT " Cy:" | |
| 390 MOVII compass_DY,mpr | |
| 391 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required | |
| 392 output_16 | |
| 393 STRCAT " Cz:" | |
| 394 MOVII compass_DZ,mpr | |
| 395 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required | |
| 582 | 396 output_16 |
| 628 | 397 STRCAT_PRINT " " |
| 398 | |
| 399 WIN_TINY .0,.104 | |
| 400 STRCPY "Ax:" | |
| 401 MOVII accel_DX,mpr | |
| 402 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required | |
| 582 | 403 output_16 |
| 628 | 404 STRCAT " Ay:" |
| 405 MOVII accel_DY,mpr | |
| 406 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required | |
| 407 output_16 | |
| 408 STRCAT " Az:" | |
| 409 MOVII accel_DZ,mpr | |
| 410 call TFT_convert_signed_16bit ; convert lo:hi into signed-short and add '-' to POSTINC2 if required | |
| 582 | 411 output_16 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
412 STRCAT_PRINT " " |
| 582 | 413 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
414 |
| 623 | 415 TFT_show_timeout_testmode: |
| 628 | 416 WIN_TINY .0,.68 |
| 582 | 417 STRCPY "T:" |
| 623 | 418 movff isr_timeout_timer,lo |
| 582 | 419 bsf leftbind |
| 628 | 420 output_8 ; display remaining time |
| 582 | 421 bcf leftbind |
| 422 STRCAT_PRINT "s " | |
| 423 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
424 |
| 628 | 425 TFT_compass_show_gain: ; show the current compass gain |
| 426 ; movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) | |
| 582 | 427 ; tstfsz lo |
| 628 | 428 ; return ; do not show unless gain=0 |
| 429 WIN_TINY .0,.86 | |
| 582 | 430 STRCPY_TEXT tCompassGain |
| 628 | 431 movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) |
| 582 | 432 bsf leftbind |
| 433 output_8 | |
| 434 bcf leftbind | |
| 435 STRCAT_PRINT "" | |
| 436 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
437 |
| 628 | 438 ;----------------------------------------------------------------------------- |
| 623 | 439 |
| 582 | 440 global TFT_surface_compass_mask |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
441 TFT_surface_compass_mask: |
| 582 | 442 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
|
443 call TFT_standard_color |
| 628 | 444 STRCPY_TEXT_PRINT tHeading ; print "Heading:" |
| 582 | 445 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
446 |
| 623 | 447 |
| 628 | 448 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
|
449 TFT_dive_compass_mask: |
| 582 | 450 WIN_FRAME_STD dm_custom_compass_graph_row, dm_custom_compass_graph_row+dm_custom_compass_graph_height, .0, .159 |
| 451 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
452 |
| 623 | 453 |
| 582 | 454 global TFT_surface_compass_heading |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
455 TFT_surface_compass_heading: |
| 582 | 456 rcall compass_heading_common |
| 457 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
|
458 call TFT_standard_color |
| 628 | 459 TFT_surface_compass_heading_com: ; show "000° N" |
| 460 movff compass_heading_new+1,WREG ; get upper byte of actual heading | |
| 461 btfsc WREG,7 ; compass calibrated? | |
| 462 bra TFT_compass_uncalibrated ; NO | |
| 463 MOVII compass_heading_shown,mpr ; get heading to be shown | |
| 464 rcall TFT_compass_helper ; show heading and its cardinal | |
| 465 btfsc divemode ; in dive mode? | |
| 466 return ; YES - done for dive mode | |
| 467 ; in surface mode - shall show bearing? | |
| 468 btfss compass_bearing_set ; is a bearing set? | |
| 469 return ; NO - done | |
| 470 btfsc compass_menu ; is the "set bearing" selection shown? | |
| 471 return ; YES - done | |
| 472 ; show bearing | |
| 473 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row | |
| 474 call TFT_attention_color | |
| 475 MOVII compass_bearing,mpr ; get bearing | |
| 476 ;bra TFT_compass_helper ; show number and cardinal and return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
477 |
| 628 | 478 TFT_compass_helper: |
| 582 | 479 bsf leftbind |
| 628 | 480 output_16dp .2 ; result is "0.000" in buffer |
| 582 | 481 bcf leftbind |
| 482 ; rearrange figures to "000" | |
| 483 movff buffer+2,buffer+0 | |
| 484 movff buffer+3,buffer+1 | |
| 485 movff buffer+4,buffer+2 | |
| 486 lfsr FSR2,buffer+3 | |
| 487 STRCAT "° " | |
| 628 | 488 rcall tft_compass_cardinal ; add cardinal to POSTINC2 |
| 489 STRCAT_PRINT "" ; finalize output | |
| 490 return ; done | |
| 491 | |
| 492 TFT_compass_uncalibrated: | |
| 493 STRCAT_PRINT "---°" ; print "---°" | |
| 494 return ; done | |
| 382 | 495 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
496 |
| 582 | 497 global TFT_dive_compass_heading |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
498 TFT_dive_compass_heading: |
| 582 | 499 rcall compass_heading_common |
| 628 | 500 WIN_FONT FT_SMALL ; set font size |
| 501 | |
| 623 | 502 ; ; ### for development only, hard-coding the bearing ### |
| 582 | 503 ; ; 244° : SW - W |
| 628 | 504 ; MOVLI .244,xA ; xA used as temp |
| 505 ; 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
|
506 |
| 623 | 507 MOVII compass_heading_shown,xA |
| 508 ; 160° viewing angle: add +360 offset if xA <= 292 for non-negative scale | |
| 509 MOVLI .292,sub_a | |
| 510 MOVII xA, sub_b | |
| 628 | 511 call subU16 ; sub_c = sub_a - sub_b |
| 512 btfsc neg_flag ; xA > 292 ? | |
| 513 bra TFT_dive_compass_heading_1 ; YES | |
| 514 ADDLI .360,xA ; NO - add offset | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
515 TFT_dive_compass_heading_1: |
| 628 | 516 SUBLI .80,xA ; subtract 80 (left pixel offset from the center) |
| 517 MOVII xA,xRD ; save result to xRD | |
| 518 ADDLI .160,xA ; add 160 (display with in pixels) | |
| 519 MOVII xA,xRDr ; save result to xRDr | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
520 |
| 628 | 521 btfss compass_bearing_set ; is a bearing set? |
| 522 bra TFT_dive_compass_ruler ; NO - skip next calculations | |
| 523 MOVII xRDr,sub_a ; YES - calculate xRD180 = xRDr - 180 | |
| 623 | 524 MOVLI .180,sub_b |
| 628 | 525 call subU16 ; sub_c = sub_a - sub_b |
| 623 | 526 MOVII sub_c,xRD180 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
527 |
| 258 | 528 TFT_dive_compass_bearing_1: |
| 582 | 529 ; calculate bearing position and visibility (ahead or behind) |
| 628 | 530 bcf compass_bearing_vis ; default is not-visible |
| 531 bcf compass_bearing_ahd ; default is behind | |
| 623 | 532 MOVII compass_bearing,xA |
| 628 | 533 ADDLI .360,xA ; calculate the bearing virtual display offset |
| 534 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
|
535 |
| 623 | 536 ; check if bearing is ahead |
| 628 | 537 MOVII divA,sub_a ; load the bearing offset into sub_a |
| 538 MOVII xRD, sub_b ; load the display offset back to sub_b | |
| 582 | 539 rcall TFT_dive_compass_bearing_ap |
| 628 | 540 btfsc compass_bearing_vis ; bearing visible? |
| 541 bra TFT_dive_compass_bearing_dir; YES | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
542 |
| 628 | 543 ; check if it is ahead with an upper turn |
| 544 MOVII divA,sub_a ; load the bearing offset into sub_a | |
| 545 MOVII xRD, sub_b ; load the display offset back to sub_b | |
| 623 | 546 ADDLI .360,sub_b |
| 582 | 547 rcall TFT_dive_compass_bearing_ap |
| 628 | 548 btfsc compass_bearing_vis ; bearing visible? |
| 549 bra TFT_dive_compass_bearing_dir; YES | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
550 |
| 628 | 551 ; check if it is ahead with a lower turn |
| 552 MOVII divA,sub_a ; load the bearing offset into sub_a | |
| 623 | 553 ADDLI .360,sub_a |
| 628 | 554 MOVII xRD, sub_b ; load the display offset back to sub_b |
| 582 | 555 rcall TFT_dive_compass_bearing_ap |
| 628 | 556 btfsc compass_bearing_vis ; bearing visible? |
| 557 bra TFT_dive_compass_bearing_dir; YES | |
| 258 | 558 |
| 623 | 559 ; marker is not ahead of us, check if it is behind of us |
| 582 | 560 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display |
| 628 | 561 MOVII xRD180,sub_a ; load the display offset back to sub_a |
| 562 MOVII divA, sub_b ; load the marker's offset into sub_b | |
| 582 | 563 rcall TFT_dive_compass_bearing_bp |
| 628 | 564 btfsc compass_bearing_vis ; bearing behind of us? |
| 565 bra TFT_dive_compass_bearing_dir; YES | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
566 |
| 628 | 567 ; check if it is behind with the upper turn |
| 623 | 568 MOVII xRD180,sub_a |
| 569 MOVII divA, sub_b | |
| 570 ADDLI .360, sub_b | |
| 571 rcall TFT_dive_compass_bearing_bp | |
| 628 | 572 btfsc compass_bearing_vis ; bearing behind of us? |
| 573 bra TFT_dive_compass_bearing_dir; YES | |
| 623 | 574 |
| 628 | 575 ; check if it is behind with the lower turn |
| 623 | 576 MOVII xRD180,sub_a |
| 577 ADDLI .360, sub_a | |
| 628 | 578 MOVII divA, sub_b ; load the marker's offset into sub_b |
| 582 | 579 rcall TFT_dive_compass_bearing_bp |
| 580 bra TFT_dive_compass_bearing_dir | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
581 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
582 TFT_dive_compass_bearing_ap: |
| 582 | 583 ; xCM received in sub_a |
| 584 ; xRD received in sub_b | |
| 585 ; 1/a. check if it's viewable from the left side | |
| 628 | 586 call subU16 ; sub_c = sub_a - sub_b |
| 587 btfsc neg_flag ; xRD > divA ? | |
| 588 return ; NO - done | |
| 589 MOVII sub_c,xC ; YES - store the RO=RP-RD for drawing | |
| 623 | 590 ; 1/b. check if it's viewable from the right side |
| 628 | 591 ADDLI .2,sub_a ; avoid thin mess on the side of the display |
| 592 ADDLI .158,sub_b ; load the display offset right side into sub_b | |
| 593 call subU16 ; sub_c = sub_a - sub_b | |
| 594 btfss neg_flag ; xRDr > xA(+2) ? | |
| 595 return ; NO - done | |
| 596 ; YES - print the bearing lines on the screen | |
| 582 | 597 movff xC+0,xCM |
| 628 | 598 bsf compass_bearing_vis ; set visible |
| 599 bsf compass_bearing_ahd ; set ahead | |
| 600 return ; done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
601 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
602 TFT_dive_compass_bearing_bp: |
| 582 | 603 ; use the (160 - (xRD180 - xCM)) formula to see if it's on the display |
| 604 ; the marker's offset received in sub_b | |
| 605 ; the xRD180 display offset received in sub_a | |
| 606 ; xRD180 - xCM | |
| 628 | 607 call subU16 ; sub_c = sub_a - sub_b |
| 608 btfsc neg_flag ; CM > xRD180 ? | |
| 609 return ; NO - not on screen, done | |
| 610 ; YES - check 160 - (X) | |
| 611 MOVLI .158, sub_a ; 158 to avoid thin mess on the side of the display | |
| 623 | 612 MOVII sub_c,sub_b |
| 628 | 613 call subU16 ; sub_c = sub_a - sub_b |
| 614 btfsc neg_flag ; X > 160 ? | |
| 615 return ; NO - not on screen, done | |
| 623 | 616 ; check if not overflow - this sounds like a double check... |
| 628 | 617 tstfsz sub_c+1 ; high byte = 0 ? |
| 618 return ; NO - sub_c must be > 160 then, done | |
| 619 movlw d'158' ; YES - load a 158 | |
| 620 cpfslt sub_c+0 ; - low byte < 158 ? | |
| 621 return ; NO - done | |
| 622 movff sub_c+0,xCM ; YES - print the bearing lines on the screen | |
| 623 bsf compass_bearing_vis ; - flag to show bearing lines | |
| 624 return ; - done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
625 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
626 TFT_dive_compass_bearing_dir: |
| 582 | 627 ; check if bearing to heading, and calculate the direction |
| 628 bcf compass_bearing_eq | |
| 629 btfss compass_bearing_vis | |
| 630 bra TFT_dive_compass_bearing_lr | |
| 631 btfss compass_bearing_ahd | |
| 632 bra TFT_dive_compass_bearing_lr | |
| 633 movff xCM,xA+0 | |
| 634 movlw d'80' | |
| 635 cpfseq xA+0 | |
| 636 bra TFT_dive_compass_bearing_lr | |
| 637 bsf compass_bearing_eq | |
| 628 | 638 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
|
639 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
640 TFT_dive_compass_bearing_lr: |
| 582 | 641 ; get the bearing virtual display offset |
| 623 | 642 MOVII compass_bearing,xA |
| 643 ; xA = xA > 292 ? xA : xA+360 | |
| 644 MOVLI .292,sub_a | |
| 645 MOVII xA, sub_b | |
| 628 | 646 call subU16 ; sub_c = sub_a - sub_b |
| 647 btfsc neg_flag ; xA > 292 ? | |
| 648 bra TFT_dive_compass_bearing_lr_1; YES | |
| 649 ADDLI .360,xA ; NO - add 360 | |
| 623 | 650 |
| 258 | 651 TFT_dive_compass_bearing_lr_1: |
| 582 | 652 ; 1. calculate whether bearing is to left or to right |
| 628 | 653 bsf compass_bearing_lft ; to the left by default |
| 582 | 654 ; xC: save center value to compare the direction to front value |
| 623 | 655 MOVII xA,xC |
| 582 | 656 ; xB: we need the left side for comparison... left = -180 |
| 623 | 657 MOVII xA, sub_a |
| 658 MOVLI .180,sub_b | |
| 628 | 659 call subU16 ; sub_c = sub_a - sub_b |
| 660 MOVII sub_c,xB ; xB has the left side of the 180° distance center | |
| 623 | 661 ; xA = xRD > (xC+100) ? RD-280 : xRD+80 |
| 662 MOVII xC, sub_a | |
| 663 ADDLI .100,sub_a | |
| 664 MOVII xRD, sub_b | |
| 628 | 665 call subU16 ; sub_c = sub_a - sub_b |
| 666 btfsc neg_flag ; xRD > xC + 100 ? | |
| 667 bra TFT_dive_compass_bearing_lr_2; YES - xA = xRD - 280 | |
| 668 ; NO - xA = xRD + 80 | |
| 623 | 669 MOVII xRD,xA |
| 670 ADDLI .80,xA | |
| 582 | 671 bra TFT_dive_compass_bearing_lr_c |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
672 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
673 TFT_dive_compass_bearing_lr_2: |
| 623 | 674 MOVII xRD,sub_a |
| 675 MOVLI .280,sub_b | |
| 628 | 676 call subU16 ; sub_c = sub_a - sub_b |
| 623 | 677 MOVII sub_c,xA |
| 582 | 678 ;bra TFT_dive_compass_bearing_lr_c |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
679 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
680 TFT_dive_compass_bearing_lr_c: |
| 582 | 681 ; xB < xA < xC => right, otherwise left (default) |
| 623 | 682 MOVII xA,sub_b |
| 683 MOVII xB,sub_a | |
| 628 | 684 call subU16 ; sub_c = sub_a - sub_b |
| 685 btfss neg_flag ; xA > xB ? | |
| 686 bra TFT_dive_compass_ruler ; NO - xB >= xA, keep default left | |
| 623 | 687 MOVII xA,sub_a |
| 688 MOVII xC,sub_b | |
| 628 | 689 call subU16 ; sub_c = sub_a - sub_b |
| 690 btfss neg_flag ; xC > xA ? | |
| 691 bra TFT_dive_compass_ruler ; NO - xA >= xC, keep default left | |
| 582 | 692 bcf compass_bearing_lft |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
693 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
694 TFT_dive_compass_ruler: |
| 582 | 695 ; calculate mod15 for the ticks |
| 623 | 696 MOVII xRD,xA |
| 697 MOVLI .15,xB | |
| 628 | 698 call div16x16 ; xA/xB=xC with xA+0 as remainder |
| 623 | 699 ; check the remainder |
| 582 | 700 movlw d'0' |
| 628 | 701 cpfsgt xA+0 ; mod15 > 0 ? |
| 702 bra TFT_dive_compass_ruler_1 ; NO - RM = 0 | |
| 703 ; YES - RM = 15 - RDmod15 | |
| 582 | 704 movlw d'15' |
| 623 | 705 subfwb xA+0,F |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
706 TFT_dive_compass_ruler_1: |
| 628 | 707 movff xA+0,lo ; xA+0 holds the RM, store it to 'lo' |
| 708 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
|
709 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
710 TFT_dive_compass_ruler_loop: |
| 623 | 711 ; 1. check if we run out of the display |
| 628 | 712 movlw d'159' ; looks like 159 works because TFT_box limits the display |
| 582 | 713 cpfslt lo,1 |
| 628 | 714 bra TFT_dive_compass_ruler_lend ; xRM >= W |
| 582 | 715 ; 2. Clear the tick area from DD to RM - in segments to avoid blinking |
| 716 ; don't do a clear if we are at 0 (zero) otherwise it will blink | |
| 717 ; because of the width underflow | |
| 718 movlw d'0' | |
| 719 cpfsgt lo,1 | |
| 720 bra TFT_dive_compass_ruler_loop_zz | |
| 721 rcall TFT_dive_compass_clr_ruler | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
722 TFT_dive_compass_ruler_loop_zz: |
| 582 | 723 ; 3. Draw the markers @ RM |
| 724 ; we receive RM in lo and DD in hi | |
| 725 movlw d'2' | |
| 628 | 726 movwf win_bargraph ; set with of ticks |
| 582 | 727 movwf win_width+0 |
| 728 clrf win_width+1 | |
| 628 | 729 movff lo,win_leftx2 ; 0..159 |
| 730 movlw dm_custom_compass_tick_top_top | |
| 731 movwf win_top ; set position for upper ticks | |
| 582 | 732 movlw dm_custom_compass_tick_height |
| 628 | 733 movwf win_height ; set hight of ticks |
| 734 call TFT_standard_color ; set color | |
| 735 call TFT_box ; draw tick | |
| 736 movlw dm_custom_compass_tick_bot_top | |
| 737 movwf win_top ; set position for lower ticks | |
| 738 movlw dm_custom_compass_tick_height | |
| 739 movwf win_height ; set hight of ticks | |
| 740 call TFT_standard_color ; color in WREG is trashed, must be set again! | |
| 741 call TFT_box ; draw tick | |
| 742 ; 4. If D < 82 and RM > 79: means we put something over the center line, | |
| 743 ; so redraw the center line | |
| 582 | 744 movlw d'82' |
| 745 cpfslt hi,1 | |
| 746 bra TFT_dive_compass_ruler_loop_zz2 | |
| 747 movlw d'79' | |
| 748 cpfsgt lo,1 | |
| 749 bra TFT_dive_compass_ruler_loop_zz2 | |
| 623 | 750 ; enough to print center line as bearing marker is not in the ticker area |
| 628 | 751 rcall TFT_dive_compass_c ; draw center line in yellow |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
752 TFT_dive_compass_ruler_loop_zz2: |
| 582 | 753 ; 5. set D = RM + 2 : position after the 2px tick |
| 754 movff lo,hi | |
| 755 movlw d'2' | |
| 756 addwf hi,F | |
| 757 ; 6. set RM = RM + 15 : position to the next tick | |
| 758 movlw d'15' | |
| 759 addwf lo,F | |
| 760 ; 7. loop | |
| 761 bra TFT_dive_compass_ruler_loop | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
762 |
| 628 | 763 TFT_dive_compass_ruler_lend: ; loop end |
| 764 ; 8. clear the rest of the tick area if D < 160 | |
| 582 | 765 movlw d'160' |
| 766 cpfslt hi | |
| 628 | 767 bra TFT_dive_compass_labels ; D >= W |
| 582 | 768 ; 9. position left to end of display to clear the remaining area |
| 769 movlw d'159' | |
| 770 movwf lo | |
| 771 ; 10. clear it | |
| 772 rcall TFT_dive_compass_clr_ruler | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
773 |
| 628 | 774 TFT_dive_compass_labels: |
| 582 | 775 ; done with the compass ruler, put the labels on the screen |
| 628 | 776 clrf hi ; hi stores the display position |
| 777 movff hi,xHI ; bank-safe clear of xHI | |
| 778 clrf lo ; lo stores the last item's display position | |
| 779 movff lo,xLO ; bank-safe clear of xLO | |
| 780 MOVLI .219,sub_a ; position of the cardinal | |
| 781 MOVII xRD, sub_b ; get the RD back to sub_b | |
| 782 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 783 btfss compass_show_cardinal ; shall show cardinal? | |
| 784 bra dcr_1 ; NO | |
| 785 STRCPY_TEXT_PRINT tSW ; YES - print it | |
| 582 | 786 dcr_1: |
| 628 | 787 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 788 MOVLI .267,sub_a ; position of the cardinal | |
| 789 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 790 btfss compass_show_cardinal ; shall show cardinal? | |
| 791 bra dcr_2 ; NO | |
| 792 STRCPY_TEXT_PRINT tW ; YES - print it | |
| 582 | 793 dcr_2: |
| 628 | 794 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 795 MOVLI .309,sub_a ; position of the cardinal | |
| 796 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 797 btfss compass_show_cardinal ; shall show cardinal? | |
| 798 bra dcr_3 ; NO | |
| 799 STRCPY_TEXT_PRINT tNW ; YES - print it | |
| 582 | 800 dcr_3: |
| 628 | 801 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 802 MOVLI .358,sub_a ; position of the cardinal | |
| 803 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 804 btfss compass_show_cardinal ; shall show cardinal? | |
| 805 bra dcr_4 ; NO | |
| 806 STRCPY_TEXT_PRINT tN ; YES - print it | |
| 582 | 807 dcr_4: |
| 628 | 808 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 809 MOVLI .399,sub_a ; position of the cardinal | |
| 810 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 811 btfss compass_show_cardinal ; shall show cardinal? | |
| 812 bra dcr_5 ; NO | |
| 813 STRCPY_TEXT_PRINT tNE ; YES - print it | |
| 582 | 814 dcr_5: |
| 628 | 815 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 816 MOVLI .448,sub_a ; position of the cardinal | |
| 817 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 818 btfss compass_show_cardinal ; shall show cardinal? | |
| 819 bra dcr_6 ; NO | |
| 820 STRCPY_TEXT_PRINT tE ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
821 dcr_6: |
| 628 | 822 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 823 MOVLI .489,sub_a ; position of the cardinal | |
| 824 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 825 btfss compass_show_cardinal ; shall show cardinal? | |
| 826 bra dcr_7 ; NO | |
| 827 STRCPY_TEXT_PRINT tSE ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
828 dcr_7: |
| 628 | 829 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 830 MOVLI .538,sub_a ; position of the cardinal | |
| 831 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 832 btfss compass_show_cardinal ; shall show cardinal? | |
| 833 bra dcr_8 ; NO | |
| 834 STRCPY_TEXT_PRINT tS ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
835 dcr_8: |
| 628 | 836 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 837 MOVLI .579,sub_a ; position of the cardinal | |
| 838 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 839 btfss compass_show_cardinal ; shall show cardinal? | |
| 840 bra dcr_9 ; NO | |
| 841 STRCPY_TEXT_PRINT tSW ; YES - print it | |
| 582 | 842 dcr_9: |
| 628 | 843 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 844 MOVLI .627,sub_a ; position of the cardinal | |
| 845 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 846 btfss compass_show_cardinal ; shall show cardinal? | |
| 847 bra dcr_10 ; NO | |
| 848 STRCPY_TEXT_PRINT tW ; YES - print it | |
| 582 | 849 dcr_10: |
| 628 | 850 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 851 MOVLI .669,sub_a ; position of the cardinal | |
| 852 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 853 btfss compass_show_cardinal ; shall show cardinal? | |
| 854 bra dcr_11 ; NO | |
| 855 STRCPY_TEXT_PRINT tNW ; YES - print it | |
| 582 | 856 dcr_11: |
| 628 | 857 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 858 MOVLI .718,sub_a ; position of the cardinal | |
| 859 rcall TFT_dive_compass_label_proc ; check if the cardinal shall be on screen | |
| 860 btfss compass_show_cardinal ; shall show? | |
| 861 bra dcr_12 ; NO | |
| 862 STRCPY_TEXT_PRINT tN ; YES - print it | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
863 dcr_12: |
| 628 | 864 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
|
865 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
866 TFT_dive_compass_label_end: |
| 628 | 867 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 582 | 868 ; restore lo and hi for the final cleanup |
| 628 | 869 movff xLO,lo ; xLO and xHI are stored in bank isr_backup |
| 582 | 870 movff xHI,hi |
| 628 | 871 ; clear the rest of the SQ area if there is more space |
| 582 | 872 movlw d'159' |
| 873 cpfslt hi | |
| 628 | 874 bra TFT_dive_compass_label_end2 ; D >= 160, no more space |
| 582 | 875 ; position left to end of display to clear the remaining area |
| 876 movlw d'158' | |
| 877 movwf lo | |
| 878 ; clear it | |
| 879 rcall TFT_dive_compass_clr_label | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
880 TFT_dive_compass_label_end2: |
| 628 | 881 rcall TFT_dive_compass_c_mk ; check if cardinal is on the center line or the marker |
| 582 | 882 ; do we have bearing set? |
| 628 | 883 btfsc compass_bearing_set ; bearing set? |
| 884 bra TFT_dive_compass_dir_text ; YES - print the direction (<< or >>) | |
| 885 rcall TFT_dive_compass_dir_lclr ; NO - clear the area (e.g. we had but removed) | |
| 582 | 886 rcall TFT_dive_compass_dir_rclr |
| 887 bra TFT_dive_compass_text | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
888 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
889 TFT_dive_compass_dir_text: |
| 582 | 890 ; bearing set, but does it point to heading? |
| 891 btfss compass_bearing_eq | |
| 628 | 892 bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the direction |
| 893 rcall TFT_dive_compass_dir_lclr ; bearing == heading - no need for direction markers | |
| 582 | 894 rcall TFT_dive_compass_dir_rclr |
| 895 bra TFT_dive_compass_text | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
896 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
897 TFT_dive_compass_dir_text_2: |
| 582 | 898 movlw color_green |
| 899 call TFT_set_color | |
| 900 btfsc compass_bearing_lft | |
| 628 | 901 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
|
902 ;TFT_dive_compass_text_rdir: |
| 582 | 903 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row-.2 |
| 904 STRCPY_PRINT ">>" | |
| 628 | 905 rcall TFT_dive_compass_dir_lclr ; do not forget to clear the left |
| 582 | 906 bra TFT_dive_compass_text |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
907 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
908 TFT_dive_compass_dir_ldir: |
| 582 | 909 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2 |
| 910 STRCPY_PRINT "<<" | |
| 628 | 911 rcall TFT_dive_compass_dir_rclr ; do not forget to clear the right |
| 582 | 912 ;bra TFT_dive_compass_text |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
913 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
914 TFT_dive_compass_text: |
| 582 | 915 ; Clear some unused space on the right mH |
| 916 WIN_BOX_BLACK dm_custom_compass_tick_top_bot+.1,dm_custom_compass_tick_bot_top-.1,.158,.159 ; top, bottom, left, right | |
| 371 | 917 |
| 582 | 918 ; Text output |
| 919 call TFT_standard_color | |
| 920 WIN_SMALL dm_custom_compass_head_column, dm_custom_compass_head_row | |
| 628 | 921 rcall TFT_surface_compass_heading_com ; show "xxx° N" |
| 582 | 922 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
923 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
924 TFT_dive_compass_dir_lclr: |
| 582 | 925 WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row-.2 |
| 926 STRCPY_PRINT " " | |
| 927 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
928 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
929 TFT_dive_compass_dir_rclr: |
| 582 | 930 WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row-.2 |
| 931 STRCPY_PRINT " " | |
| 932 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
933 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
934 TFT_dive_compass_label_proc: |
| 582 | 935 movlw d'14' |
| 628 | 936 movwf up ; cardinal width in px |
| 623 | 937 bcf compass_show_cardinal |
| 582 | 938 ; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ? |
| 939 ; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP): | |
| 623 | 940 MOVII xRD,sub_b |
| 628 | 941 call subU16 ; sub_c = sub_a - sub_b |
| 942 btfsc neg_flag ; >= 0 ? | |
| 943 return ; NO | |
| 582 | 944 ; store the RO=RP-RD for drawing |
| 623 | 945 MOVII sub_c,xC |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
946 |
| 582 | 947 ; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160 |
| 948 ; if already above, no need to process the rest of the labels | |
| 628 | 949 movff up,WREG ; take care about the width |
| 582 | 950 addwf sub_a+0,1 |
| 951 btfsc STATUS, C | |
| 952 incf sub_a+1 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
953 |
| 623 | 954 MOVII xRDr,sub_b |
| 628 | 955 call subU16 ; sub_c = sub_a - sub_b |
| 956 btfss neg_flag ; < 0 ? | |
| 957 bra TFT_dive_compass_label_end ; NO | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
958 |
| 582 | 959 ; 2. restore RO=RP-RD from 1/a. |
| 960 movff xC+0,lo | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
961 |
| 582 | 962 ; 3. Clear the segment from DD(hi) to lo |
| 963 ; don't do a clear if we are at 0 (zero) otherwise it will blink | |
| 964 ; ?because of the width underflow? | |
| 965 movlw d'1' | |
| 966 cpfsgt lo | |
| 967 bra TFT_dive_compass_label_proc_p | |
| 968 rcall TFT_dive_compass_clr_label | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
969 TFT_dive_compass_label_proc_p: |
| 582 | 970 ; 4. print the SQ on the screen |
| 971 call TFT_standard_color | |
| 623 | 972 bsf compass_show_cardinal |
|
257
5dd0f39d05d4
minor speed and size improvements for the compass routine
heinrichsweikamp
parents:
256
diff
changeset
|
973 ;TFT_dive_compass_label_print: |
| 582 | 974 movlw dm_custom_compass_label_row |
| 975 movff WREG,win_top | |
| 976 movff lo,win_leftx2 | |
| 623 | 977 WIN_FONT FT_SMALL |
| 582 | 978 ; 6. retain the new display positions |
| 979 movff lo,hi | |
| 980 movff up,WREG | |
| 981 addwf hi,F | |
| 982 movff lo,xLO | |
| 983 movff hi,xHI | |
| 984 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
985 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
986 TFT_dive_compass_c_mk: |
| 582 | 987 ; Common task to draw center line and marker |
| 988 ; until a proper implementation make it simple: | |
| 989 rcall TFT_dive_compass_mk | |
| 628 | 990 TFT_dive_compass_c: |
| 582 | 991 movlw color_yellow |
| 992 WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81 ; center line in yellow | |
| 993 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
994 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
995 TFT_dive_compass_mk: |
| 582 | 996 ; draw the bearing on the screen if visible and if we just put something over it |
| 628 | 997 btfss compass_bearing_set ; bearing set? |
| 998 return ; NO - done | |
| 999 btfss compass_bearing_vis ; YES - bearing visible? | |
| 1000 return ; NO - bearing set but not visible, done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1001 |
| 582 | 1002 ; save lo/hi from trashing |
| 623 | 1003 MOVII mpr,xA |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1004 |
| 582 | 1005 ; did we just update the marker's position? |
| 1006 ; DD.......DD | |
| 1007 ; CM+2>=DD(old) or CM-2<=DD | |
| 1008 ; ToDo | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1009 |
| 582 | 1010 btfss compass_bearing_ahd |
| 1011 bra TFT_dive_compass_mk_rear | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1012 ;TFT_dive_compass_mk_front: |
| 582 | 1013 clrf lo |
| 1014 movff xCM,lo | |
| 628 | 1015 bsf compass_show_cardinal ; set=green marker |
| 582 | 1016 rcall TFT_dive_compass_mk_print |
| 623 | 1017 bcf compass_show_cardinal |
| 582 | 1018 bra TFT_dive_compass_mk_end |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1019 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1020 TFT_dive_compass_mk_rear: |
| 582 | 1021 clrf lo |
| 1022 movff xCM,lo | |
| 628 | 1023 bcf compass_show_cardinal ; set=red marker |
| 582 | 1024 rcall TFT_dive_compass_mk_print |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1025 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1026 TFT_dive_compass_mk_end: |
| 623 | 1027 MOVII xA,mpr |
| 582 | 1028 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1029 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1030 TFT_dive_compass_mk_print: |
| 582 | 1031 movlw d'1' |
| 1032 cpfsgt lo | |
| 628 | 1033 bra TFT_dive_compass_mk_print_2 ; lo <= 1, skip the first line |
| 582 | 1034 movlw d'2' |
| 1035 subwf lo,0 | |
| 1036 ; movff WREG,win_leftx2 | |
| 1037 rcall TFT_dive_compass_mk_print_3 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1038 TFT_dive_compass_mk_print_2: |
| 582 | 1039 ; save hi/lo |
| 623 | 1040 MOVII mpr,divA |
| 582 | 1041 ; clear the middle of the bearing marker |
| 1042 movff lo,hi | |
| 1043 movlw d'2' | |
| 1044 addwf lo,1 | |
| 1045 rcall TFT_dive_compass_clr_label | |
| 1046 ; restore hi/lo | |
| 623 | 1047 MOVII divA,mpr |
| 582 | 1048 ; print a dot on the middle |
| 623 | 1049 movf lo,W |
| 582 | 1050 rcall TFT_dive_compass_mk_print_dot |
| 1051 ; finally print the right marker line | |
| 1052 movlw d'2' | |
| 1053 addwf lo,0 | |
| 1054 ; rcall TFT_dive_compass_mk_print_3 | |
| 1055 ; return | |
|
257
5dd0f39d05d4
minor speed and size improvements for the compass routine
heinrichsweikamp
parents:
256
diff
changeset
|
1056 TFT_dive_compass_mk_print_3: |
| 582 | 1057 movwf win_leftx2 |
| 1058 movlw dm_custom_compass_label_row | |
| 1059 movwf win_top | |
| 1060 movlw dm_custom_compass_label_height-.2 | |
| 1061 movwf win_height | |
| 1062 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
|
1063 TFT_dive_compass_mk_print_dot: |
| 582 | 1064 movwf win_leftx2 |
| 1065 movlw dm_custom_compass_label_row + .9 | |
| 1066 movwf win_top | |
| 1067 movlw d'4' | |
| 1068 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
|
1069 TFT_dive_compass_mk_print_4: |
| 582 | 1070 movlw .158 |
| 1071 cpfslt win_leftx2 | |
| 1072 bra TFT_dive_compass_mk_print_5 | |
| 1073 movlw d'2' | |
| 623 | 1074 movwf win_bargraph |
| 582 | 1075 movwf win_width+0 |
| 1076 clrf win_width+1 | |
| 1077 movlw color_green | |
| 623 | 1078 btfss compass_show_cardinal |
| 582 | 1079 movlw color_red |
| 1080 call TFT_set_color | |
| 1081 call TFT_box | |
| 371 | 1082 TFT_dive_compass_mk_print_5: |
| 582 | 1083 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1084 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1085 TFT_dive_compass_clr_label: |
| 582 | 1086 movlw dm_custom_compass_label_row-.2 ; set top & height |
| 623 | 1087 movwf win_top |
| 582 | 1088 movlw dm_custom_compass_label_height+.2 |
| 623 | 1089 movwf win_height |
| 582 | 1090 rcall TFT_dive_compass_clear |
| 1091 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1092 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1093 TFT_dive_compass_clr_ruler: |
| 582 | 1094 ; top tick |
| 1095 movlw dm_custom_compass_tick_top_top ; set top & height | |
| 623 | 1096 movwf win_top |
| 582 | 1097 movlw dm_custom_compass_tick_height |
| 623 | 1098 movwf win_height |
| 582 | 1099 rcall TFT_dive_compass_clear |
| 1100 ;bottom tick | |
| 1101 movlw dm_custom_compass_tick_bot_top ; set top & height | |
| 623 | 1102 movwf win_top |
| 582 | 1103 movlw dm_custom_compass_tick_height |
| 623 | 1104 movwf win_height |
| 582 | 1105 ; rcall TFT_dive_compass_clear |
| 1106 ; return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1107 TFT_dive_compass_clear: |
| 582 | 1108 ; we receive RM in lo and DD in hi |
| 1109 ; calculate width = RM-D | |
| 623 | 1110 movf hi,W |
| 582 | 1111 subwf lo,W |
| 623 | 1112 bz TFT_dive_compass_clear3 ; do nothing if there is nothing to do |
| 582 | 1113 movwf win_width+0 ; RM-DD |
| 1114 movwf win_bargraph | |
| 1115 clrf win_width+1 | |
| 1116 movlw .1 | |
| 1117 cpfsgt win_width+0 | |
| 623 | 1118 bra TFT_dive_compass_clear3 ; do not clear a single pixel (or less) |
| 582 | 1119 movff hi,win_leftx2 |
| 1120 movlw color_black | |
| 1121 call TFT_set_color | |
| 1122 call TFT_box | |
| 370 | 1123 TFT_dive_compass_clear3: |
| 582 | 1124 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1125 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1126 tft_compass_cardinal: |
| 623 | 1127 btfsc hi,0 ; heading > 255° ? |
| 1128 bra tft_compass_cardinal2 ; YES - must be W, NW or N | |
| 1129 ; NO - must be W, SW, S, SE, E, NE or N | |
| 582 | 1130 movlw .23 |
| 1131 subwf lo,W | |
| 1132 btfss STATUS,C | |
| 1133 bra tft_compass_cardinal_N | |
| 1134 movlw .68 | |
| 1135 subwf lo,W | |
| 1136 btfss STATUS,C | |
| 1137 bra tft_compass_cardinal_NE | |
| 1138 movlw .113 | |
| 1139 subwf lo,W | |
| 1140 btfss STATUS,C | |
| 1141 bra tft_compass_cardinal_E | |
| 1142 movlw .158 | |
| 1143 subwf lo,W | |
| 1144 btfss STATUS,C | |
| 1145 bra tft_compass_cardinal_SE | |
| 1146 movlw .203 | |
| 1147 subwf lo,W | |
| 1148 btfss STATUS,C | |
| 1149 bra tft_compass_cardinal_S | |
| 1150 movlw .248 | |
| 1151 subwf lo,W | |
| 1152 btfss STATUS,C | |
| 1153 bra tft_compass_cardinal_SW | |
| 1154 bra tft_compass_cardinal_W | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1155 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1156 tft_compass_cardinal2: |
| 582 | 1157 movlw .37 |
| 1158 subwf lo,W | |
| 1159 btfss STATUS,C | |
| 1160 bra tft_compass_cardinal_W | |
| 1161 movlw .82 | |
| 1162 subwf lo,W | |
| 1163 btfss STATUS,C | |
| 1164 bra tft_compass_cardinal_NW | |
| 1165 ; bra tft_compass_cardinal_N | |
| 1166 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1167 tft_compass_cardinal_N: |
| 582 | 1168 STRCAT_TEXT tN |
| 1169 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1170 tft_compass_cardinal_NE: |
| 582 | 1171 STRCAT_TEXT tNE |
| 1172 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1173 tft_compass_cardinal_E: |
| 582 | 1174 STRCAT_TEXT tE |
| 1175 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1176 tft_compass_cardinal_SE: |
| 582 | 1177 STRCAT_TEXT tSE |
| 1178 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1179 tft_compass_cardinal_S: |
| 582 | 1180 STRCAT_TEXT tS |
| 1181 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1182 tft_compass_cardinal_SW: |
| 582 | 1183 STRCAT_TEXT tSW |
| 1184 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1185 tft_compass_cardinal_W: |
| 582 | 1186 STRCAT_TEXT tW |
| 1187 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1188 tft_compass_cardinal_NW: |
| 582 | 1189 STRCAT_TEXT tNW |
| 1190 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1191 |
| 628 | 1192 ;----------------------------------------------------------------------------- |
| 623 | 1193 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1194 compass_heading_common: |
| 623 | 1195 btfss compass_enabled ; compass enabled? |
| 628 | 1196 bra compass_heading_common_zero ; NO |
| 623 | 1197 |
| 628 | 1198 ; Get an averaged new heading |
| 623 | 1199 movlw compass_averaging ; number of averaging cycles |
| 1200 movwf up ; initialize loop counter | |
| 628 | 1201 compass_heading_common_1: |
| 623 | 1202 call I2C_RX_compass ; test compass |
| 1203 call I2C_RX_accelerometer ; test accelerometer | |
| 1204 call compass_filter ; filter raw compass + accelerometer readings | |
| 1205 decfsz up,F ; decrement loop counter, done? | |
| 628 | 1206 bra compass_heading_common_1 ; NO - loop |
| 623 | 1207 |
| 628 | 1208 call compass ; do compass correction (C-code) |
| 623 | 1209 banksel common ; back to bank common |
| 1210 | |
| 628 | 1211 ; Check for calibration and change |
| 1212 MOVII compass_heading_shown,sub_a ; transfer shown heading to sub_a | |
| 1213 MOVII compass_heading_new, sub_b ; transfer new heading to sub_b | |
| 623 | 1214 |
| 1215 btfsc sub_b+1,7 ; valid compass calibration? | |
| 628 | 1216 bra compass_heading_common_zero ; NO |
| 1217 | |
| 1218 movf sub_a+0,W ; get shown heading, low byte | |
| 1219 cpfseq sub_b+0 ; compare with new heading, low byte, equal? | |
| 1220 bra compass_heading_common_2 ; NO - not equal | |
| 1221 movf sub_a+1,W ; get shown heading, high byte | |
| 1222 cpfseq sub_b+1 ; compare with new heading, high byte, equal? | |
| 1223 bra compass_heading_common_2 ; NO - not equal | |
| 1224 return ; YES - done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1225 |
| 628 | 1226 compass_heading_common_2: |
| 1227 ; turn both headings such that compass_heading_new points to 0° | |
| 1228 call subU16 ; sub_c = compass_heading_shown - compass_heading_new | |
| 1229 btfss neg_flag ; was compass_heading_new in 1st halve? | |
| 1230 bra compass_heading_common_3 ; YES - check where compass_heading_shown is now | |
| 1231 MOVLI .360, sub_a ; NO - overturned, need to turn back to match 360° | |
| 1232 MOVII sub_c,sub_b ; - move overturned compass_heading_new to sub_b | |
| 1233 call subU16 ; - sub_c = angle between overturned compass_heading_shown and 360° | |
| 1234 compass_heading_common_3: | |
| 1235 ; check if turned compass_heading_shown is in 1st or 2nd halve | |
| 1236 MOVII sub_c,sub_a ; sub_a = turned compass_heading_shown | |
| 1237 MOVLI .180, sub_b ; sub_b = begin of 2nd halve | |
| 1238 call cmpU16 ; check (turned compass_heading_shown) - 180° | |
| 1239 btfss neg_flag ; result negative? | |
| 1240 bra compass_heading_common_5 ; NO - in 2nd halve, increment towards compass_heading_new | |
| 1241 ;bra compass_heading_common_4 ; YES - in 1st halve, decrement towards compass_heading_new | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1242 |
| 628 | 1243 compass_heading_common_4: |
| 1244 ; decrement compass_heading_shown towards compass_heading_new | |
| 1245 rcall compass_heading_stepsize_2 ; calculate step size and put it into sub_b | |
| 1246 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a | |
| 1247 call subU16 ; decrement heading: sub_c = compass_heading_shown - step size | |
| 1248 btfss neg_flag ; did an under-run occur? | |
| 1249 bra compass_heading_common_6 ; NO - store result | |
| 1250 MOVLI .360, sub_a ; YES - wrap around 360° | |
| 1251 MOVII sub_c,sub_b ; - transfer decrement result to sub_b | |
| 1252 call subU16 ; - wrap decrement result around | |
| 1253 bra compass_heading_common_6 ; - store wrapped result | |
| 1254 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1255 |
| 628 | 1256 compass_heading_common_5: |
| 1257 ; increment compass_heading_shown towards compass_heading_new | |
| 1258 rcall compass_heading_stepsize_1 ; calculate step size and put it into sub_b | |
| 1259 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a | |
| 1260 call addU16 ; increment heading: sub_c = compass_heading_shown + step size | |
| 1261 MOVII sub_c,sub_a ; transfer increment result to sub_a | |
| 1262 MOVLI .360, sub_b ; load wrap-around threshold | |
| 1263 call subU16 ; calculate if over-run occurred | |
| 1264 btfss neg_flag ; did an over-run occur? | |
| 1265 bra compass_heading_common_6 ; YES - store already wrapped-around result | |
| 1266 MOVII sub_a,sub_c ; NO - retrieve former straight increment result | |
| 1267 bra compass_heading_common_6 ; - store wrapped result | |
| 1268 | |
| 1269 compass_heading_common_zero: | |
| 1270 CLRI sub_c ; set heading to 0° | |
| 1271 ;bra compass_heading_common_6 ; store heading | |
| 1272 | |
| 1273 compass_heading_common_6: | |
| 1274 MOVII sub_c,compass_heading_shown ; store new shown heading | |
| 1275 return ; done | |
| 1276 | |
| 1277 compass_heading_stepsize_1: | |
| 1278 ; turn heading difference (180...359) into a step size | |
| 1279 MOVLI .360, sub_a ; load 360° | |
| 1280 MOVII sub_c,sub_b ; load difference | |
| 1281 call subU16 ; sub_c = 360 - difference (i.e. 1...180 now) | |
| 1282 compass_heading_stepsize_2: | |
| 1283 ; turn heading difference (1...180) into a step size | |
| 1284 bcf STATUS,C ; clear carry | |
| 1285 rrcf sub_c+0 ; heading difference /= 2 | |
| 1286 bcf STATUS,C ; clear carry | |
| 1287 rrcf sub_c+0 ; heading difference /= 2, total /= 4 now | |
| 1288 incf sub_c+0,f ; final += 1 to have one increment / decrement at least | |
| 1289 MOVII sub_c,sub_b ; transfer result to sub_b | |
| 1290 return | |
| 1291 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1292 |
| 623 | 1293 ENDIF ; _compass |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
1294 |
| 582 | 1295 END |