Mercurial > public > hwos_code
annotate src/compass_ops.asm @ 656:8af5aefbcdaf default tip
Update to 3.31 beta
| author | heinrichsweikamp |
|---|---|
| date | Thu, 27 Nov 2025 18:32:58 +0100 |
| parents | 75e90cd0c2c3 |
| children |
| rev | line source |
|---|---|
| 582 | 1 ;============================================================================= |
| 2 ; | |
| 634 | 3 ; File compass_ops.asm combined next generation V3.09.5 |
| 582 | 4 ; |
| 5 ; Compass Operations | |
| 6 ; | |
| 654 | 7 ; Copyright (c) 2011, JD Gascuel, heinrichs weikamp gmbh, all right reserved. |
| 582 | 8 ;============================================================================= |
| 9 | |
| 634 | 10 #include "hwos.inc" |
| 11 #include "i2c.inc" | |
| 12 #include "tft_outputs.inc" | |
| 13 #include "tft.inc" | |
| 14 #include "strings.inc" | |
| 15 #include "wait.inc" | |
| 16 #include "surfmode.inc" | |
| 17 #include "divemode.inc" | |
| 18 #include "math.inc" | |
| 19 #include "convert.inc" | |
| 20 #include "start.inc" | |
| 21 #include "colorschemes.inc" | |
| 147 | 22 |
| 0 | 23 |
| 623 | 24 IFDEF _compass |
| 25 | |
| 26 | |
| 27 ; local flags | |
| 656 | 28 ;#DEFINE compass_show_cardinal compass_flags,0 ; =1: show the cardinal (N, NE, E, ...) |
| 29 ;#DEFINE compass_bearing_eq compass_flags,1 ; =1: bearing is in direction, do not show << or >> | |
| 30 ;#DEFINE compass_bearing_lft compass_flags,2 ; =1: bearing is to the left/<<, =0: to the right/>> | |
| 31 ;#DEFINE compass_bearing_vis compass_flags,3 ; =1: bearing is visible (either ahead or behind/-180°) | |
| 32 ;#DEFINE compass_bearing_ahd compass_flags,4 ; =1: bearing is ahead, =0: behind | |
| 33 ;; compass_flags,5 ; --- unused | |
| 34 ;; compass_flags,6 ; --- unused | |
| 35 ;; compass_flags,7 ; --- unused | |
| 623 | 36 |
| 37 | |
| 0 | 38 ; Make sure symbols from the .inc are available to the C code: |
| 623 | 39 |
| 40 ; filtered data - Compass | |
| 582 | 41 global compass_DX_f |
| 42 global compass_DY_f | |
| 43 global compass_DZ_f | |
| 0 | 44 |
| 623 | 45 ; filtered Data - Accelerometer |
| 582 | 46 global accel_DX_f |
| 47 global accel_DY_f | |
| 48 global accel_DZ_f | |
| 49 | |
| 623 | 50 ; Calibration Data |
| 582 | 51 global compass_CX_f |
| 52 global compass_CY_f | |
| 53 global compass_CZ_f | |
| 0 | 54 |
| 623 | 55 ; Temporary Values to pass Q15 Arithmetics around |
| 582 | 56 global compass_a |
| 57 global compass_b | |
| 0 | 58 |
| 582 | 59 ; Result |
| 623 | 60 global compass_heading_new |
| 0 | 61 |
| 147 | 62 |
| 582 | 63 extern compass |
| 64 extern compass_reset_calibration | |
| 65 extern compass_add_calibration | |
| 66 extern compass_solve_calibration | |
| 623 | 67 |
| 631 | 68 extern option_check_and_store_all |
| 582 | 69 |
|
337
508d7fb98b34
cleanup menu, add "Auto SP" option (Not working yet), minor layout change in compass menu
heinrichsweikamp
parents:
300
diff
changeset
|
70 |
| 634 | 71 ;============================================================================= |
| 72 compass_ops1 CODE | |
| 73 ;============================================================================= | |
| 582 | 74 |
| 75 | |
| 0 | 76 ;----------------------------------------------------------------------------- |
| 77 ; Filter compass values | |
| 78 ; | |
| 79 ; Apply linear filtering to input parameters. | |
| 80 | |
| 582 | 81 ; Apply filtering formula: reg_f += (reg - reg_f) / 4 |
| 82 FILTER16 MACRO reg, reg_f | |
| 83 movf reg_f+0,W | |
| 84 subwf reg+0,W | |
| 85 movwf PRODL | |
| 86 movf reg_f+1,W | |
| 87 subwfb reg+1,W | |
| 88 rcall filter_16_common | |
| 89 addwf reg_f+0,F | |
| 90 movf PRODH,W | |
| 91 addwfc reg_f+1,F | |
| 92 ENDM | |
| 148 | 93 |
| 94 filter_16_common: | |
| 582 | 95 movwf PRODH |
| 623 | 96 bcf STATUS,C ; copy sign bit into carry |
| 582 | 97 btfsc PRODH,7 |
| 98 bsf STATUS,C | |
| 623 | 99 rrcf PRODH,F ; 16 bit shift right |
| 582 | 100 rrcf PRODL,F |
| 623 | 101 bcf STATUS,C ; copy sign bit into carry |
| 582 | 102 btfsc PRODH,7 |
| 103 bsf STATUS,C | |
| 623 | 104 rrcf PRODH,F ; 16 bit shift right |
| 582 | 105 rrcf PRODL,W |
| 106 return | |
| 0 | 107 |
| 623 | 108 |
| 582 | 109 global compass_filter |
| 0 | 110 compass_filter: |
| 623 | 111 banksel compass_DX ; select bank common2 |
| 582 | 112 FILTER16 compass_DX, compass_DX_f |
| 113 FILTER16 compass_DY, compass_DY_f | |
| 114 FILTER16 compass_DZ, compass_DZ_f | |
| 115 FILTER16 accel_DX, accel_DX_f | |
| 116 FILTER16 accel_DY, accel_DY_f | |
| 117 FILTER16 accel_DZ, accel_DZ_f | |
| 623 | 118 banksel common ; back to bank common |
| 582 | 119 return |
| 0 | 120 |
| 121 | |
| 634 | 122 ;============================================================================= |
| 123 compass_ops2 CODE | |
| 124 ;============================================================================= | |
| 0 | 125 |
| 126 ;----------------------------------------------------------------------------- | |
| 127 ; Q15 fractional numbers: a * b / 2**16 (UNSIGNED) | |
| 128 ; | |
| 623 | 129 ; Uses 16x16->16 multiply, for positive integers, keeping only the most |
| 130 ; relevant bits. | |
| 0 | 131 ; |
| 132 ; Used to multiply two Q15 numbers, in the range 0..1, | |
| 133 ; represented as 0..32767, that is a / 2**15. | |
| 134 ; | |
| 135 ; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14. | |
| 136 ; So to get back a Q15 number, we need a shift-left... | |
| 623 | 137 |
| 138 global compass_umul ; called from compass.c | |
| 0 | 139 compass_umul: |
| 623 | 140 banksel compass_a ; select bank common2 |
| 582 | 141 rcall compass_mul_16 |
| 0 | 142 |
| 143 ; The 2x time, by left-shifting inserting the missing bit: | |
| 144 compass_mul_2: | |
| 623 | 145 rlcf compass_r+2,F ; missing bit into carry |
| 582 | 146 rlcf compass_r+0,F |
| 147 rlcf compass_r+1,F | |
| 623 | 148 MOVII compass_r,PROD ; return value into PROD |
| 582 | 149 return |
| 0 | 150 |
| 151 ; The 16x16-> multiply: | |
| 152 compass_mul_16: | |
| 623 | 153 movf compass_a+1,W ; block ah*bh |
| 582 | 154 mulwf compass_b+1 |
| 155 movff PRODL,compass_r+0 ; and copy | |
| 156 movff PRODH,compass_r+1 | |
| 0 | 157 |
| 623 | 158 movf compass_a+0,W ; block al*bl |
| 159 mulwf compass_b+0 | |
| 160 movff PRODH,compass_r+2 ; into fraction byte | |
| 0 | 161 |
| 623 | 162 movf compass_a+1,W ; block ah*bl |
| 582 | 163 mulwf compass_b+0 |
| 164 movf PRODL,W | |
| 623 | 165 addwf compass_r+2,F ; fraction part to carry |
| 582 | 166 movf PRODH,W ; and add16 |
| 167 addwfc compass_r+0,F | |
| 168 movlw 0 | |
| 628 | 169 addwfc compass_r+1,F |
| 0 | 170 |
| 623 | 171 movf compass_a+0,W ; block al*bh |
| 582 | 172 mulwf compass_b+1 |
| 173 movf PRODL,W | |
| 623 | 174 addwf compass_r+2,F ; fraction part to carry |
| 582 | 175 movf PRODH,W ; and add16 |
| 176 addwfc compass_r+0,F | |
| 177 movlw 0 | |
| 178 addwfc compass_r+1,F | |
| 0 | 179 |
| 582 | 180 return |
| 0 | 181 |
| 182 ;----------------------------------------------------------------------------- | |
| 183 ; Q15 fractional numbers: a * b / 2**16 (SIGNED) | |
| 184 | |
| 623 | 185 global compass_imul ; called from compass.c |
| 0 | 186 compass_imul: |
| 623 | 187 banksel compass_a ; select bank common2 |
| 582 | 188 rcall compass_mul_16 |
| 0 | 189 |
| 582 | 190 btfss compass_b+1,7 |
| 191 bra compass_mul_3 | |
| 0 | 192 |
| 582 | 193 movf compass_a+0,W |
| 194 subwf compass_r+0,F | |
| 195 movf compass_a+1,W | |
| 196 subwfb compass_r+1,F | |
| 0 | 197 |
| 198 compass_mul_3: | |
| 582 | 199 btfss compass_a+1,7 |
| 200 bra compass_mul_4 | |
| 0 | 201 |
| 582 | 202 movf compass_b+0,W |
| 203 subwf compass_r+0,F | |
| 204 movf compass_b+1,W | |
| 623 | 205 subwfb compass_r+1,F |
| 0 | 206 |
| 207 compass_mul_4: | |
| 628 | 208 bcf compass_r+1,6 ; copy bit 7 to 6, so keep it after 2x |
| 582 | 209 btfsc compass_r+1,7 |
| 210 bsf compass_r+1,6 | |
| 211 bra compass_mul_2 | |
| 0 | 212 |
| 623 | 213 |
| 634 | 214 ;============================================================================= |
| 215 compass_ops3 CODE | |
| 216 ;============================================================================= | |
| 217 | |
| 218 global compass_calibration | |
| 219 compass_calibration: ; compass calibration | |
| 628 | 220 bsf block_sensor_interrupt ; disable sensor interrupts |
| 221 call I2C_sleep_compass ; stop compass | |
| 634 | 222 call TFT_ClearScreen ; clear the screen |
| 582 | 223 ; Mask |
| 634 | 224 WIN_STD .16,.0 ; set position |
| 656 | 225 FONT_COLOR color_green ; set font color |
| 634 | 226 STRCPY_TEXT_PRINT tCompassCalibration |
| 227 btfss switch_right2 ; wait until button (with a moving magnet in some OSTC!) is released | |
| 623 | 228 bra $-2 |
| 147 | 229 |
| 582 | 230 ; WIN_SMALL .0,.215 |
| 634 | 231 ; FONT_COLOR color_standard |
| 582 | 232 ; STRCPY_TEXT_PRINT tExit |
| 233 WAITMS d'255' | |
| 234 WAITMS d'255' | |
| 147 | 235 |
| 628 | 236 call request_speed_fastest ; request CPU speed change to fastest speed |
| 623 | 237 |
| 643 | 238 movlw .5 ; initialize gain |
| 634 | 239 movff WREG,opt_compass_gain ; ... |
| 428 | 240 |
| 628 | 241 movlw .60 ; initialize timeout to 60 seconds |
| 242 movwf isr_timeout_reload ; copy WREG to isr_timeout_reload | |
| 634 | 243 bsf restart_timeout ; request ISR to restart the timeout |
| 628 | 244 bcf trigger_timeout ; clear any pending timeout trigger |
| 245 compass_calibration_gainset: ; reduce the gain, set bank here! | |
| 246 banksel opt_compass_gain ; select bank options table | |
| 247 decf opt_compass_gain,F ; reduce by one | |
| 248 btfsc STATUS,N ; < 0 ? | |
| 249 clrf opt_compass_gain ; YES - keep at zero | |
| 250 btfsc STATUS,N ; < 0 ? | |
| 634 | 251 bra compass_calibration_loop1 ; YES - skip gain stuff (Would hang here in case of compass failure) |
| 628 | 252 banksel common ; bank to bank common |
| 623 | 253 |
| 634 | 254 call I2C_init_compass ; initialize compass chip |
| 582 | 255 |
| 628 | 256 btfsc compass_type3 ; compass type 3 ? |
| 257 bra compass_calibration_loop1 ; YES - skip gain stuff | |
| 258 btfsc compass_type2 ; compass type 2 ? | |
| 259 bra compass_calibration_loop1 ; YES - skip gain stuff | |
| 582 | 260 |
| 628 | 261 rcall TFT_compass_show_gain ; show the current compass gain |
| 147 | 262 |
| 582 | 263 WAITMS d'250' |
| 628 | 264 WAITMS d'250' ; wait for first reading... |
| 147 | 265 |
| 628 | 266 movlw .60 ; calibration shall run for 60 seconds |
| 634 | 267 call restart_timeout_time ; restart the timeout |
| 147 | 268 |
| 628 | 269 call I2C_RX_compass ; read compass |
| 270 call I2C_RX_accelerometer ; read accelerometer | |
| 147 | 271 |
| 582 | 272 ; Test all axes for +4096 (Hi byte=16) |
| 628 | 273 banksel compass_DX ; select bank common2 |
| 582 | 274 movlw .16 |
| 275 cpfseq compass_DX+1 | |
| 276 bra $+4 | |
| 277 bra compass_calibration_gainset | |
| 278 cpfseq compass_DY+1 | |
| 279 bra $+4 | |
| 280 bra compass_calibration_gainset | |
| 281 cpfseq compass_DZ+1 | |
| 282 bra $+4 | |
| 283 bra compass_calibration_gainset | |
| 147 | 284 |
| 582 | 285 ; Test all axes for -4096 (Hi byte=240) |
| 286 movlw .240 | |
| 287 cpfseq compass_DX+1 | |
| 288 bra $+4 | |
| 289 bra compass_calibration_gainset | |
| 290 cpfseq compass_DY+1 | |
| 291 bra $+4 | |
| 292 bra compass_calibration_gainset | |
| 293 cpfseq compass_DZ+1 | |
| 294 bra $+4 | |
| 295 bra compass_calibration_gainset | |
| 147 | 296 |
| 628 | 297 compass_calibration_loop1: ; done with gain |
| 298 banksel common ; bank to bank common | |
| 299 rcall compass_filter_init ; set DX_f values | |
| 300 call compass_reset_calibration ; reset CX_f values (C-code) | |
| 301 banksel common ; back to bank common | |
| 147 | 302 |
| 303 compass_calibration_loop2: | |
| 628 | 304 call I2C_RX_compass ; read compass |
| 305 call I2C_RX_accelerometer ; test accelerometer | |
| 634 | 306 call compass_filter ; filter compass raw data |
| 147 | 307 |
| 582 | 308 ; Twice |
| 628 | 309 call I2C_RX_compass ; read compass |
| 310 call I2C_RX_accelerometer ; test accelerometer | |
| 634 | 311 call compass_filter ; filter compass raw data |
| 582 | 312 |
| 628 | 313 ; btfsc compass_type1 ; compass1? |
| 314 ; bra compass_calibration_loop3 ; YES - skip gain stuff | |
| 147 | 315 |
| 645 | 316 banksel opt_compass_gain ; select bank options table |
| 317 tstfsz opt_compass_gain ; =0? | |
| 318 bra $+4 ; No | |
| 319 bra compass_calibration_loop3 ; YES - skip gain stuff | |
| 582 | 320 ; Test all axes for +4096 (Hi byte=16) |
| 628 | 321 banksel compass_DX ; select bank common2 |
| 582 | 322 movlw .16 |
| 323 cpfseq compass_DX+1 | |
| 324 bra $+4 | |
| 325 bra compass_calibration_gainset | |
| 326 cpfseq compass_DY+1 | |
| 327 bra $+4 | |
| 328 bra compass_calibration_gainset | |
| 329 cpfseq compass_DZ+1 | |
| 330 bra $+4 | |
| 331 bra compass_calibration_gainset | |
| 147 | 332 |
| 582 | 333 ; Test all axes for -4096 (Hi byte=240) |
| 334 movlw .240 | |
| 335 cpfseq compass_DX+1 | |
| 336 bra $+4 | |
| 337 bra compass_calibration_gainset | |
| 338 cpfseq compass_DY+1 | |
| 339 bra $+4 | |
| 340 bra compass_calibration_gainset | |
| 341 cpfseq compass_DZ+1 | |
| 342 bra $+4 | |
| 343 bra compass_calibration_gainset | |
| 147 | 344 |
| 428 | 345 compass_calibration_loop3: |
| 645 | 346 banksel common ; back to bank common |
| 623 | 347 ; and register only one value out of four: |
| 628 | 348 call compass_add_calibration ; check and store new max/min values (C-code) |
| 349 banksel common ; back to bank common | |
| 147 | 350 |
| 628 | 351 rcall TFT_compass_fast ; show values |
| 352 btfsc trigger_timeout ; timeout (calibration done)? | |
| 353 bra compass_calibration_exit ; YES - done | |
| 354 btfss trigger_full_second ; NO - new second begun? | |
| 631 | 355 bra compass_calibration_loop2 ; NO - loop |
| 628 | 356 bcf trigger_full_second ; YES - clear flag |
| 357 rcall TFT_show_timeout_testmode ; - show remaining time | |
| 358 bra compass_calibration_loop2 ; - loop | |
| 147 | 359 |
| 360 compass_calibration_exit: | |
| 628 | 361 bcf block_sensor_interrupt ; re-enable sensor interrupts |
| 623 | 362 |
| 628 | 363 call compass_solve_calibration ; calculate calibration factors (C-code) |
| 364 banksel common ; back to bank common | |
| 365 | |
| 366 call request_speed_normal ; request CPU speed change to normal speed | |
| 623 | 367 |
| 634 | 368 bsf option_changed ; flag that option values have changed |
| 631 | 369 bsf restart_fast ; request to skip logos and waits on restart |
| 370 | |
| 371 movlw .6 ; coding for surface compass view | |
| 372 movff WREG,customview_surfmode ; set to compass view to show | |
| 373 | |
| 374 goto restart ; done | |
| 623 | 375 |
| 147 | 376 |
| 634 | 377 ;----------------------------------------------------------------------------- |
| 378 ; Helper Function | |
| 379 ; | |
| 380 compass_filter_init: | |
| 381 MOVII compass_DX,compass_DX_f | |
| 382 MOVII compass_DY,compass_DY_f | |
| 383 MOVII compass_DZ,compass_DZ_f | |
| 628 | 384 |
| 634 | 385 MOVII accel_DX,accel_DX_f |
| 386 MOVII accel_DY,accel_DY_f | |
| 387 MOVII accel_DZ,accel_DZ_f | |
| 582 | 388 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
389 |
| 634 | 390 ;----------------------------------------------------------------------------- |
| 391 ; Show Calibration Progress | |
| 392 ; | |
| 393 TFT_compass_fast: | |
| 650 | 394 WIN_TINY .5,.50 ; set font and position |
| 634 | 395 FONT_COLOR_MEMO ; set font color |
| 396 | |
| 397 STRCPY "Cx:" ; print label | |
| 398 MOVII compass_DX,mpr ; get value | |
| 399 rcall TFT_compass_fast_helper ; print value | |
| 400 | |
| 401 STRCAT " Cy:" ; print label | |
| 402 MOVII compass_DY,mpr ; get value | |
| 403 rcall TFT_compass_fast_helper ; print value | |
| 404 | |
| 405 STRCAT " Cz:" ; print label | |
| 406 MOVII compass_DZ,mpr ; get value | |
| 407 rcall TFT_compass_fast_helper ; print value | |
| 408 | |
| 409 PRINT ; dump to screen | |
| 410 | |
| 650 | 411 WIN_TINY .5,.104 ; set font and position |
| 634 | 412 FONT_COLOR_MEMO ; set font color |
| 413 | |
| 414 STRCPY "Ax:" ; print label | |
| 415 MOVII accel_DX,mpr ; get value | |
| 416 rcall TFT_compass_fast_helper ; print value | |
| 417 | |
| 418 STRCAT " Ay:" ; print label | |
| 419 MOVII accel_DY,mpr ; get value | |
| 420 rcall TFT_compass_fast_helper ; print value | |
| 421 | |
| 422 STRCAT " Az:" ; print label | |
| 423 MOVII accel_DZ,mpr ; get value | |
| 424 rcall TFT_compass_fast_helper ; print value | |
| 425 | |
| 426 PRINT ; dump to screen | |
| 427 return ; done | |
| 428 | |
| 429 | |
| 430 TFT_compass_fast_helper: | |
| 431 call convert_signed_16bit ; convert lo:hi into unsigned-short and add '-' to POSTINC2 if required | |
| 432 btfsc neg_flag ; is value negative? | |
| 433 bra TFT_compass_fast_helper1; YES - print value | |
| 434 PUTC '+' ; NO - print a plus sign first | |
| 435 TFT_compass_fast_helper1: | |
| 436 output_65535 ; print value (0-65535) | |
| 582 | 437 return |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
438 |
| 634 | 439 |
| 440 | |
| 441 ;----------------------------------------------------------------------------- | |
| 442 ; Show remaining Calibration Time | |
| 443 ; | |
| 444 TFT_show_timeout_testmode: | |
| 445 WIN_TINY .0,.68 ; set font and position | |
| 446 FONT_COLOR_MEMO ; set font color | |
| 447 STRCPY "T:" ; print label | |
| 448 movff isr_timeout_timer,lo ; get remaining time | |
| 449 output_256 ; print remaining time (xxx) | |
| 450 PUTC_PRINT "s" ; append unit and dump to screen | |
| 451 return ; done | |
| 452 | |
| 453 | |
| 454 ;----------------------------------------------------------------------------- | |
| 455 ; Show current Compass Gain | |
| 456 ; | |
| 628 | 457 TFT_compass_show_gain: ; show the current compass gain |
| 458 ; movff opt_compass_gain,lo ; 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) | |
| 582 | 459 ; tstfsz lo |
| 634 | 460 ; return ; do not show unless gain > 0 |
| 461 WIN_TINY .0,.86 ; set font and position | |
| 462 FONT_COLOR_MEMO ; set font color | |
| 463 STRCPY_TEXT tCompassGain ; print label | |
| 464 movff opt_compass_gain,lo ; get value: 0-7 (230 LSB/Gauss to 1370 LSB/Gauss) | |
| 465 output_99 ; print value (xx) | |
| 466 PRINT ; dump to screen | |
| 467 return ; done | |
| 468 | |
| 469 | |
| 470 ;============================================================================= | |
| 471 compass_ops4 CODE | |
| 472 ;============================================================================= | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
473 |
| 628 | 474 ;----------------------------------------------------------------------------- |
| 634 | 475 ; Mask for Compass in Surface Mode |
| 476 ; | |
| 582 | 477 global TFT_surface_compass_mask |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
478 TFT_surface_compass_mask: |
| 582 | 479 WIN_SMALL surf_compass_mask_column,surf_compass_mask_row |
| 656 | 480 FONT_COLOR color_green ; set font color |
| 628 | 481 STRCPY_TEXT_PRINT tHeading ; print "Heading:" |
| 634 | 482 return ; done |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
483 |
| 623 | 484 |
| 634 | 485 ;----------------------------------------------------------------------------- |
| 486 ; Compass Display in Surface Mode | |
| 487 ; | |
| 582 | 488 global TFT_surface_compass_heading |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
489 TFT_surface_compass_heading: |
| 634 | 490 call compass_heading_common ; compute heading |
| 582 | 491 WIN_STD surf_compass_head_column,surf_compass_head_row |
| 634 | 492 |
| 628 | 493 TFT_surface_compass_heading_com: ; show "000° N" |
| 634 | 494 FONT_COLOR_MEMO ; set default / dive-mode standard color |
| 628 | 495 movff compass_heading_new+1,WREG ; get upper byte of actual heading |
| 496 btfsc WREG,7 ; compass calibrated? | |
| 497 bra TFT_compass_uncalibrated ; NO | |
| 498 MOVII compass_heading_shown,mpr ; get heading to be shown | |
| 499 rcall TFT_compass_helper ; show heading and its cardinal | |
| 500 btfsc divemode ; in dive mode? | |
| 501 return ; YES - done for dive mode | |
| 502 ; in surface mode - shall show bearing? | |
| 503 btfss compass_bearing_set ; is a bearing set? | |
| 504 return ; NO - done | |
| 505 btfsc compass_menu ; is the "set bearing" selection shown? | |
| 506 return ; YES - done | |
| 507 ; show bearing | |
| 508 WIN_SMALL surf_compass_bear_column,surf_compass_bear_row | |
| 634 | 509 FONT_COLOR color_yellow ; set font color |
| 628 | 510 MOVII compass_bearing,mpr ; get bearing |
| 511 ;bra TFT_compass_helper ; show number and cardinal and return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
512 |
| 628 | 513 TFT_compass_helper: |
| 634 | 514 output_999 ; print bearing |
| 515 STRCAT "° " ; append unit and a space | |
| 516 call tft_compass_cardinal ; append cardinal buffer | |
| 517 PRINT ; dump to screen | |
| 628 | 518 return ; done |
| 519 | |
| 520 TFT_compass_uncalibrated: | |
| 521 STRCAT_PRINT "---°" ; print "---°" | |
| 522 return ; done | |
| 382 | 523 |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
524 |
| 634 | 525 ;============================================================================= |
| 526 compass_ops5 CODE | |
| 527 ;============================================================================= | |
| 528 | |
| 529 | |
| 530 ;----------------------------------------------------------------------------- | |
| 531 ; Mask for Compass in Dive Mode | |
| 532 ; | |
| 656 | 533 global TFT_dive_compass_mask |
| 634 | 534 TFT_dive_compass_mask: |
| 656 | 535 FONT_COLOR_MASK ; select color |
| 536 WIN_TINY dm_custom_compass_mask_column,dm_custom_compass_mask_row | |
| 537 STRCPY_TEXT_PRINT tCompassMenu ; print label | |
| 634 | 538 return |
| 539 | |
| 540 | |
| 541 ;----------------------------------------------------------------------------- | |
| 542 ; Compass Display in Dive Mode | |
| 543 ; | |
| 582 | 544 global TFT_dive_compass_heading |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
545 TFT_dive_compass_heading: |
| 634 | 546 call compass_heading_common ; compute heading |
| 656 | 547 ; Text output |
| 548 FONT_COLOR_MEMO ; set default / dive-mode standard color | |
| 549 WIN_MEDIUM dm_custom_compass_head_column, dm_custom_compass_head_row | |
| 550 MOVII compass_heading_shown,mpr ; get heading to be shown | |
| 551 bsf leading_zeros | |
| 552 output_999 ; print bearing | |
| 553 bcf leading_zeros | |
| 554 PRINT ; dump to screen | |
| 555 WIN_STD dm_custom_compass_unit_column, dm_custom_compass_unit_row | |
| 556 STRCAT "°" | |
| 557 call tft_compass_cardinal ; append cardinal buffer | |
| 558 PRINT | |
| 559 ; shall show bearing? | |
| 628 | 560 btfss compass_bearing_set ; is a bearing set? |
| 656 | 561 return ; NO - done |
| 562 ; show bearing | |
| 563 WIN_SMALL dm_compass_bear_column,dm_compass_bear_row | |
| 564 FONT_COLOR color_yellow ; set font color | |
| 565 MOVII compass_bearing,mpr ; get bearing | |
| 566 goto TFT_compass_helper ; show number and cardinal and return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
567 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
568 tft_compass_cardinal: |
| 623 | 569 btfsc hi,0 ; heading > 255° ? |
| 570 bra tft_compass_cardinal2 ; YES - must be W, NW or N | |
| 571 ; NO - must be W, SW, S, SE, E, NE or N | |
| 582 | 572 movlw .23 |
| 573 subwf lo,W | |
| 574 btfss STATUS,C | |
| 575 bra tft_compass_cardinal_N | |
| 576 movlw .68 | |
| 577 subwf lo,W | |
| 578 btfss STATUS,C | |
| 579 bra tft_compass_cardinal_NE | |
| 580 movlw .113 | |
| 581 subwf lo,W | |
| 582 btfss STATUS,C | |
| 583 bra tft_compass_cardinal_E | |
| 584 movlw .158 | |
| 585 subwf lo,W | |
| 586 btfss STATUS,C | |
| 587 bra tft_compass_cardinal_SE | |
| 588 movlw .203 | |
| 589 subwf lo,W | |
| 590 btfss STATUS,C | |
| 591 bra tft_compass_cardinal_S | |
| 592 movlw .248 | |
| 593 subwf lo,W | |
| 594 btfss STATUS,C | |
| 595 bra tft_compass_cardinal_SW | |
| 596 bra tft_compass_cardinal_W | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
597 |
|
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
598 tft_compass_cardinal2: |
| 582 | 599 movlw .37 |
| 600 subwf lo,W | |
| 601 btfss STATUS,C | |
| 602 bra tft_compass_cardinal_W | |
| 603 movlw .82 | |
| 604 subwf lo,W | |
| 605 btfss STATUS,C | |
| 606 bra tft_compass_cardinal_NW | |
| 607 ; bra tft_compass_cardinal_N | |
| 608 | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
609 tft_compass_cardinal_N: |
| 582 | 610 STRCAT_TEXT tN |
| 611 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
612 tft_compass_cardinal_NE: |
| 582 | 613 STRCAT_TEXT tNE |
| 614 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
615 tft_compass_cardinal_E: |
| 582 | 616 STRCAT_TEXT tE |
| 617 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
618 tft_compass_cardinal_SE: |
| 582 | 619 STRCAT_TEXT tSE |
| 620 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
621 tft_compass_cardinal_S: |
| 582 | 622 STRCAT_TEXT tS |
| 623 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
624 tft_compass_cardinal_SW: |
| 582 | 625 STRCAT_TEXT tSW |
| 626 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
627 tft_compass_cardinal_W: |
| 582 | 628 STRCAT_TEXT tW |
| 629 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
630 tft_compass_cardinal_NW: |
| 582 | 631 STRCAT_TEXT tNW |
| 632 return | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
633 |
| 634 | 634 ;============================================================================= |
| 635 compass_ops6 CODE | |
| 636 ;============================================================================= | |
| 637 | |
| 628 | 638 ;----------------------------------------------------------------------------- |
| 634 | 639 ; get and process heading |
| 640 ; | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
641 compass_heading_common: |
| 623 | 642 btfss compass_enabled ; compass enabled? |
| 628 | 643 bra compass_heading_common_zero ; NO |
| 623 | 644 |
| 628 | 645 ; Get an averaged new heading |
| 623 | 646 movlw compass_averaging ; number of averaging cycles |
| 647 movwf up ; initialize loop counter | |
| 628 | 648 compass_heading_common_1: |
| 623 | 649 call I2C_RX_compass ; test compass |
| 650 call I2C_RX_accelerometer ; test accelerometer | |
| 651 call compass_filter ; filter raw compass + accelerometer readings | |
| 652 decfsz up,F ; decrement loop counter, done? | |
| 628 | 653 bra compass_heading_common_1 ; NO - loop |
| 623 | 654 |
| 628 | 655 call compass ; do compass correction (C-code) |
| 623 | 656 banksel common ; back to bank common |
| 657 | |
| 628 | 658 ; Check for calibration and change |
| 659 MOVII compass_heading_shown,sub_a ; transfer shown heading to sub_a | |
| 660 MOVII compass_heading_new, sub_b ; transfer new heading to sub_b | |
| 623 | 661 |
| 662 btfsc sub_b+1,7 ; valid compass calibration? | |
| 628 | 663 bra compass_heading_common_zero ; NO |
| 664 | |
| 665 movf sub_a+0,W ; get shown heading, low byte | |
| 666 cpfseq sub_b+0 ; compare with new heading, low byte, equal? | |
| 667 bra compass_heading_common_2 ; NO - not equal | |
| 668 movf sub_a+1,W ; get shown heading, high byte | |
| 669 cpfseq sub_b+1 ; compare with new heading, high byte, equal? | |
| 670 bra compass_heading_common_2 ; NO - not equal | |
| 671 return ; YES - done | |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
672 |
| 628 | 673 compass_heading_common_2: |
| 674 ; turn both headings such that compass_heading_new points to 0° | |
| 675 call subU16 ; sub_c = compass_heading_shown - compass_heading_new | |
| 676 btfss neg_flag ; was compass_heading_new in 1st halve? | |
| 677 bra compass_heading_common_3 ; YES - check where compass_heading_shown is now | |
| 678 MOVLI .360, sub_a ; NO - overturned, need to turn back to match 360° | |
| 679 MOVII sub_c,sub_b ; - move overturned compass_heading_new to sub_b | |
| 680 call subU16 ; - sub_c = angle between overturned compass_heading_shown and 360° | |
| 681 compass_heading_common_3: | |
| 682 ; check if turned compass_heading_shown is in 1st or 2nd halve | |
| 683 MOVII sub_c,sub_a ; sub_a = turned compass_heading_shown | |
| 684 MOVLI .180, sub_b ; sub_b = begin of 2nd halve | |
| 685 call cmpU16 ; check (turned compass_heading_shown) - 180° | |
| 686 btfss neg_flag ; result negative? | |
| 687 bra compass_heading_common_5 ; NO - in 2nd halve, increment towards compass_heading_new | |
| 688 ;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
|
689 |
| 628 | 690 compass_heading_common_4: |
| 691 ; decrement compass_heading_shown towards compass_heading_new | |
| 692 rcall compass_heading_stepsize_2 ; calculate step size and put it into sub_b | |
| 693 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a | |
| 694 call subU16 ; decrement heading: sub_c = compass_heading_shown - step size | |
| 695 btfss neg_flag ; did an under-run occur? | |
| 696 bra compass_heading_common_6 ; NO - store result | |
| 697 MOVLI .360, sub_a ; YES - wrap around 360° | |
| 698 MOVII sub_c,sub_b ; - transfer decrement result to sub_b | |
| 699 call subU16 ; - wrap decrement result around | |
| 700 bra compass_heading_common_6 ; - store wrapped result | |
| 701 | |
| 702 compass_heading_common_5: | |
| 703 ; increment compass_heading_shown towards compass_heading_new | |
| 704 rcall compass_heading_stepsize_1 ; calculate step size and put it into sub_b | |
| 705 MOVII compass_heading_shown,sub_a ; transfer unturned shown heading to sub_a | |
| 706 call addU16 ; increment heading: sub_c = compass_heading_shown + step size | |
| 707 MOVII sub_c,sub_a ; transfer increment result to sub_a | |
| 708 MOVLI .360, sub_b ; load wrap-around threshold | |
| 709 call subU16 ; calculate if over-run occurred | |
| 710 btfss neg_flag ; did an over-run occur? | |
| 711 bra compass_heading_common_6 ; YES - store already wrapped-around result | |
| 712 MOVII sub_a,sub_c ; NO - retrieve former straight increment result | |
| 713 bra compass_heading_common_6 ; - store wrapped result | |
| 714 | |
| 715 compass_heading_common_zero: | |
| 716 CLRI sub_c ; set heading to 0° | |
| 717 ;bra compass_heading_common_6 ; store heading | |
| 718 | |
| 719 compass_heading_common_6: | |
| 720 MOVII sub_c,compass_heading_shown ; store new shown heading | |
| 721 return ; done | |
| 722 | |
| 723 compass_heading_stepsize_1: | |
| 724 ; turn heading difference (180...359) into a step size | |
| 725 MOVLI .360, sub_a ; load 360° | |
| 726 MOVII sub_c,sub_b ; load difference | |
| 727 call subU16 ; sub_c = 360 - difference (i.e. 1...180 now) | |
| 728 compass_heading_stepsize_2: | |
| 729 ; turn heading difference (1...180) into a step size | |
| 730 bcf STATUS,C ; clear carry | |
| 731 rrcf sub_c+0 ; heading difference /= 2 | |
| 732 bcf STATUS,C ; clear carry | |
| 733 rrcf sub_c+0 ; heading difference /= 2, total /= 4 now | |
| 734 incf sub_c+0,f ; final += 1 to have one increment / decrement at least | |
| 735 MOVII sub_c,sub_b ; transfer result to sub_b | |
| 736 return | |
| 737 | |
| 623 | 738 ENDIF ; _compass |
|
256
5b4ef0b9090d
place compass display code into compass_ops.asm
heinrichsweikamp
parents:
214
diff
changeset
|
739 |
| 634 | 740 ;----------------------------------------------------------------------------- |
| 741 | |
| 582 | 742 END |