line source
#include "hwos.inc"+ −
#include "i2c.inc"+ −
#include "tft_outputs.inc"+ −
#include "isr.inc"+ −
#include "tft.inc"+ −
#include "strings.inc"+ −
#include "wait.inc" ; speed_*+ −
#include "surfmode.inc"+ −
#include "divemode.inc"+ −
#include "math.inc"+ −
#include "convert.inc"+ −
+ −
+ −
; Make sure symbols from the .inc are available to the C code:+ −
; Filtered data+ −
global compass_DX_f, compass_DY_f, compass_DZ_f+ −
global accel_DX_f, accel_DY_f, accel_DZ_f+ −
+ −
; Calibration data+ −
global compass_CX_f+ −
global compass_CY_f+ −
global compass_CZ_f+ −
+ −
; Tmp values to pass Q15 arithmetics around+ −
global compass_a+ −
global compass_b+ −
+ −
; Result+ −
global compass_heading; , compass_roll, compass_pitch+ −
+ −
extern compass+ −
extern compass_reset_calibration+ −
extern compass_add_calibration+ −
extern compass_solve_calibration+ −
+ −
compass_ops code+ −
;-----------------------------------------------------------------------------+ −
; Filter compass values+ −
;+ −
; Apply linear filtering to input parameters.+ −
+ −
; Apply filtering formula:+ −
; reg_f += (reg - reg_f) / 4+ −
FILTER16 MACRO reg, reg_f+ −
movf reg_f+0,W+ −
subwf reg+0,W+ −
movwf PRODL+ −
movf reg_f+1,W+ −
subwfb reg+1,W+ −
rcall filter_16_common+ −
addwf reg_f+0,F+ −
movf PRODH,W+ −
addwfc reg_f+1,F+ −
ENDM+ −
+ −
filter_16_common:+ −
movwf PRODH+ −
+ −
bcf STATUS,C ; Copy sign bit into carry+ −
btfsc PRODH,7+ −
bsf STATUS,C+ −
rrcf PRODH,F ; 16bit shift right+ −
rrcf PRODL,F+ −
+ −
bcf STATUS,C ; Copy sign bit into carry+ −
btfsc PRODH,7+ −
bsf STATUS,C+ −
rrcf PRODH,F ; 16bit shift right+ −
rrcf PRODL,W+ −
return+ −
+ −
global compass_filter+ −
compass_filter:+ −
banksel compass_DX+ −
+ −
FILTER16 compass_DX, compass_DX_f+ −
FILTER16 compass_DY, compass_DY_f+ −
FILTER16 compass_DZ, compass_DZ_f+ −
FILTER16 accel_DX, accel_DX_f+ −
FILTER16 accel_DY, accel_DY_f+ −
FILTER16 accel_DZ, accel_DZ_f+ −
banksel common+ −
return+ −
+ −
;-----------------------------------------------------------------------------+ −
+ −
compass_filter_init:+ −
movff compass_DX+0, compass_DX_f+0+ −
movff compass_DX+1, compass_DX_f+1+ −
movff compass_DY+0, compass_DY_f+0+ −
movff compass_DY+1, compass_DY_f+1+ −
movff compass_DZ+0, compass_DZ_f+0+ −
movff compass_DZ+1, compass_DZ_f+1+ −
movff accel_DX+0, accel_DX_f+0+ −
movff accel_DX+1, accel_DX_f+1+ −
movff accel_DY+0, accel_DY_f+0+ −
movff accel_DY+1, accel_DY_f+1+ −
movff accel_DZ+0, accel_DZ_f+0+ −
movff accel_DZ+1, accel_DZ_f+1+ −
return+ −
+ −
;-----------------------------------------------------------------------------+ −
; Q15 fractional numbers: a * b / 2**16 (UNSIGNED)+ −
;+ −
; Uses 16x16->16 multiply, for positiv integers, keeping only the most+ −
; revelant bits.+ −
;+ −
; Used to multiply two Q15 numbers, in the range 0..1,+ −
; represented as 0..32767, that is a / 2**15.+ −
;+ −
; (a/2**15) * (b/2**15) = a*b / 2**30 = (a*b/2**16) / 2**14.+ −
; So to get back a Q15 number, we need a shift-left...+ −
global compass_umul+ −
compass_umul:+ −
rcall compass_mul_16+ −
+ −
; The 2x time, by left-shifting inserting the missing bit:+ −
compass_mul_2:+ −
rlcf compass_r+2,F ; Missing bit into carry+ −
rlcf compass_r+0,F+ −
rlcf compass_r+1,F+ −
movff compass_r+0,PRODL ; return value into ProdH:L+ −
movff compass_r+1,PRODH+ −
return+ −
+ −
; The 16x16-> multiply:+ −
compass_mul_16:+ −
banksel compass_a+ −
+ −
movf compass_a+1,W ; Block ah*bh+ −
mulwf compass_b+1+ −
movff PRODL,compass_r+0 ; and copy+ −
movff PRODH,compass_r+1+ −
+ −
movf compass_a+0,W ; Block al*bl+ −
mulwf compass_b+0+ −
movff PRODH,compass_r+2 ; Into fraction byte+ −
+ −
movf compass_a+1,W ; Block ah*bl+ −
mulwf compass_b+0+ −
movf PRODL,W+ −
addwf compass_r+2,F ; Fraction part to carry.+ −
movf PRODH,W ; and add16+ −
addwfc compass_r+0,F+ −
movlw 0+ −
addwfc compass_r+1,F+ −
+ −
movf compass_a+0,W ; Block al*bh+ −
mulwf compass_b+1+ −
movf PRODL,W+ −
addwf compass_r+2,F ; Fraction part to carry.+ −
movf PRODH,W ; and add16+ −
addwfc compass_r+0,F+ −
movlw 0+ −
addwfc compass_r+1,F+ −
+ −
return+ −
+ −
;-----------------------------------------------------------------------------+ −
; Q15 fractional numbers: a * b / 2**16 (SIGNED)+ −
+ −
global compass_imul+ −
compass_imul:+ −
rcall compass_mul_16+ −
+ −
btfss compass_b+1,7+ −
bra compass_mul_3+ −
+ −
movf compass_a+0,W+ −
subwf compass_r+0,F+ −
movf compass_a+1,W+ −
subwfb compass_r+1,F+ −
+ −
compass_mul_3:+ −
btfss compass_a+1,7+ −
bra compass_mul_4+ −
+ −
movf compass_b+0,W+ −
subwf compass_r+0,F+ −
movf compass_b+1,W+ −
subwfb compass_r+1,F+ −
+ −
compass_mul_4:+ −
bcf compass_r+1,6 ; Copy bit 7 to 6, so keep it after 2x+ −
btfsc compass_r+1,7+ −
bsf compass_r+1,6+ −
bra compass_mul_2+ −
+ −
global compass_calibration_loop+ −
compass_calibration_loop: ; Compass calibration+ −
bsf no_sensor_int ; No Sensor ISR+ −
call I2C_sleep_accelerometer ; Stop accelerometer+ −
call I2C_sleep_compass ; Stop compass+ −
call TFT_ClearScreen+ −
; Mask+ −
WIN_COLOR color_greenish+ −
WIN_SMALL .16,.0+ −
STRCPY_TEXT_PRINT tCompassMenu+ −
btfss switch_right2 ; wait until button is released+ −
bra $-4+ −
+ −
call TFT_standard_color+ −
; WIN_SMALL .0,.215+ −
; STRCPY_TEXT_PRINT tExit+ −
WAITMS d'255'+ −
WAITMS d'255'+ −
movlw .7 ; Gain init+ −
movff WREG,opt_compass_gain+ −
compass_calibration_gainset: ; Reduce the gain, set bank here!+ −
banksel opt_compass_gain+ −
decf opt_compass_gain,F ; Reduce by one+ −
btfsc STATUS,N ; <0?+ −
clrf opt_compass_gain ; Yes, keep at zero+ −
+ −
banksel common+ −
call I2C_init_accelerometer+ −
call I2C_init_compass_fast+ −
rcall TFT_compass_show_gain ; Show the current compass gain+ −
+ −
WAITMS d'100'+ −
+ −
clrf timeout_counter2+ −
clrf timeout_counter3+ −
+ −
call speed_fastest+ −
call I2C_RX_compass ; read compass+ −
call I2C_RX_accelerometer ; read Accelerometer+ −
+ −
; Test all axes for +4096 (Hi byte=16)+ −
banksel compass_DX+1+ −
movlw .16+ −
cpfseq compass_DX+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DY+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DZ+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
+ −
; Test all axes for -4096 (Hi byte=240)+ −
movlw .240+ −
cpfseq compass_DX+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DY+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DZ+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
banksel common+ −
+ −
rcall compass_filter_init ; set DX_f values+ −
call compass_reset_calibration ; Reset CX_f values+ −
banksel common+ −
+ −
compass_calibration_loop2:+ −
call I2C_RX_compass ; read compass+ −
call I2C_RX_accelerometer ; Test Accelerometer+ −
rcall compass_filter ; Filter compass raw data+ −
banksel common+ −
+ −
; Twice+ −
call I2C_RX_compass ; read compass+ −
call I2C_RX_accelerometer ; Test Accelerometer+ −
rcall compass_filter ; Filter compass raw data+ −
banksel common+ −
+ −
; Test all axes for +4096 (Hi byte=16)+ −
banksel compass_DX+1+ −
movlw .16+ −
cpfseq compass_DX+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DY+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DZ+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
+ −
; Test all axes for -4096 (Hi byte=240)+ −
movlw .240+ −
cpfseq compass_DX+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DY+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
cpfseq compass_DZ+1+ −
bra $+4+ −
bra compass_calibration_gainset+ −
banksel common+ −
;+ −
; ; Three+ −
; call I2C_RX_compass ; read compass+ −
; call I2C_RX_accelerometer ; Test Accelerometer+ −
; call compass_filter ; Filter compass raw data+ −
; banksel common+ −
;+ −
; ; Four times to get cleaner values+ −
; call I2C_RX_compass ; read compass+ −
; call I2C_RX_accelerometer ; Test Accelerometer+ −
; call compass_filter ; Filter compass raw data+ −
+ −
; And register only one value out of four:+ −
call compass_add_calibration ; check and store new max/min values+ −
banksel common+ −
+ −
call TFT_compass_fast ; show values+ −
+ −
btfsc sleepmode ; Sleepmode active?+ −
bra compass_calibration_exit ; Yes, exit+ −
+ −
; btfsc switch_left ; Button pressed?+ −
; bra compass_calibration_exit ; Yes, exit+ −
+ −
btfss onesecupdate ; do every second tasks?+ −
bra compass_calibration_loop2 ; no, loop here+ −
+ −
movlw .60+ −
call timeout_testmode ; check timeout+ −
movlw .60+ −
rcall TFT_show_timeout_testmode ; Show the timeout+ −
+ −
bcf onesecupdate ; clear flag+ −
+ −
bra compass_calibration_loop2 ; loop here+ −
+ −
compass_calibration_exit:+ −
call compass_solve_calibration+ −
banksel common+ −
extern option_save_all+ −
call option_save_all ; save all settings into EEPROM+ −
bcf sleepmode ; Clear the flag before exiting to surfacemode+ −
movlw .6+ −
movwf customview_surfmode ; Set to compass view...+ −
goto surfloop ; ...and exit+ −
+ −
global TFT_compass_fast+ −
TFT_compass_fast:+ −
WIN_TINY .20,.50+ −
STRCPY "X:"+ −
movff compass_DX+0,lo+ −
movff compass_DX+1,hi+ −
call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required+ −
output_16+ −
STRCAT " Y:"+ −
movff compass_DY+0,lo+ −
movff compass_DY+1,hi+ −
call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required+ −
output_16+ −
STRCAT " Z:"+ −
movff compass_DZ+0,lo+ −
movff compass_DZ+1,hi+ −
call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required+ −
output_16+ −
STRCAT_PRINT " "+ −
return+ −
+ −
TFT_show_timeout_testmode: ; With timeout in WREG...+ −
movwf hi+ −
WIN_TINY .20,.68+ −
STRCPY "T:"+ −
movf timeout_counter2,W ; current timeout+ −
subwf hi,W ; subtract from timeout value+ −
addlw .1 ; +1+ −
movwf lo+ −
bsf leftbind+ −
output_8 ; Display timeout+ −
bcf leftbind+ −
STRCAT_PRINT "s "+ −
return+ −
+ −
+ −
TFT_compass_show_gain: ; Show the current compass gain+ −
movff opt_compass_gain,lo ; 0-7 (230LSB/Gauss to 1370LSB/Gaus)+ −
tstfsz lo+ −
return ; Do not show unless gain=0+ −
WIN_TINY .20,.86+ −
STRCPY_TEXT tCompassGain+ −
movff opt_compass_gain,lo ; 0-7 (230LSB/Gauss to 1370LSB/Gaus)+ −
bsf leftbind+ −
output_8+ −
bcf leftbind+ −
STRCAT_PRINT "!"+ −
return+ −
+ −
global TFT_surface_compass_mask+ −
TFT_surface_compass_mask:+ −
WIN_SMALL surf_compass_mask_column,surf_compass_mask_row+ −
call TFT_standard_color+ −
STRCPY_TEXT_PRINT tHeading ; Heading:+ −
return+ −
+ −
global TFT_dive_compass_mask+ −
TFT_dive_compass_mask:+ −
WIN_FRAME_STD dm_custom_compass_graph_row, dm_custom_compass_graph_row+dm_custom_compass_graph_height, .0, .159+ −
return+ −
+ −
global TFT_surface_compass_heading+ −
TFT_surface_compass_heading:+ −
rcall compass_heading_common+ −
WIN_STD surf_compass_head_column,surf_compass_head_row+ −
call TFT_standard_color+ −
TFT_surface_compass_heading_com: ; Show "000° N"+ −
movff compass_heading+0,lo+ −
movff compass_heading+1,hi+ −
call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required+ −
+ −
; Shown and actual identical?+ −
movff compass_heading_shown+0,WREG+ −
cpfseq lo+ −
bra TFT_surface_compass_heading_com1 ; Not equal+ −
movff compass_heading_shown+1,WREG+ −
cpfseq hi+ −
bra TFT_surface_compass_heading_com1 ; Not equal+ −
bra TFT_surface_compass_heading_com3 ; equal, skip smoothing+ −
+ −
TFT_surface_compass_heading_com1:+ −
movff lo,sub_a+0+ −
movff hi,sub_a+1+ −
movff compass_heading_shown+0,sub_b+0+ −
movff compass_heading_shown+1,sub_b+1+ −
call subU16+ −
btfsc neg_flag+ −
bra TFT_surface_compass_heading_com2 ; shown > actual+ −
; shown < actual+ −
banksel compass_heading_shown+ −
infsnz compass_heading_shown+0,F+ −
incf compass_heading_shown+1,F ; +1+ −
bra TFT_surface_compass_heading_com3+ −
+ −
TFT_surface_compass_heading_com2:+ −
banksel compass_heading_shown+ −
movlw d'1'+ −
subwf compass_heading_shown+0,F+ −
movlw d'0'+ −
subwfb compass_heading_shown+1,F ; -1+ −
+ −
TFT_surface_compass_heading_com3:+ −
banksel common+ −
movff compass_heading_shown+0,lo+ −
movff compass_heading_shown+1,hi+ −
bsf leftbind+ −
output_16dp .2 ; Result is "0.000"+ −
bcf leftbind+ −
; rearrange figures to "000"+ −
movff buffer+2,buffer+0+ −
movff buffer+3,buffer+1+ −
movff buffer+4,buffer+2+ −
lfsr FSR2,buffer+3+ −
STRCAT "° "+ −
rcall tft_compass_cardinal ; Add cardinal and ordinal to POSTINC2+ −
clrf WREG+ −
movff WREG,buffer+.7 ; limit to 7 chars+ −
STRCAT_PRINT ""+ −
return+ −
+ −
global TFT_dive_compass_heading+ −
TFT_dive_compass_heading:+ −
rcall compass_heading_common+ −
; ; ToDo - these are for development only, hardcoding the bearing position+ −
; ; 244° : SW - W+ −
; movlw low(d'244')+ −
; movff WREG,compass_bearing+0+ −
; movlw high(d'244')+ −
; movff WREG,compass_bearing+1+ −
+ −
movff compass_heading_shown+0,xA+0+ −
movff compass_heading_shown+1,xA+1+ −
; xRD and xRDlft+ −
; 1. 160° viewing angle: +360 offset if xA<=292; for non-negative scale+ −
movlw high(d'292')+ −
movff WREG,sub_a+1+ −
movlw low(d'292')+ −
movff WREG,sub_a+0+ −
movff xA+1,sub_b+1+ −
movff xA+0,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; xA>292+ −
bra TFT_dive_compass_heading_1 ;yes+ −
; no, xA<=292+ −
movlw high(d'360')+ −
addwf xA+1,1+ −
movlw low(d'360')+ −
addwf xA+0,1+ −
btfsc STATUS,C+ −
incf xA+1+ −
TFT_dive_compass_heading_1:+ −
; 2. -80: left pixel offset from the center+ −
movlw low( d'80' )+ −
subwf xA+0,1+ −
btfss STATUS,C+ −
decf xA+1+ −
; 3. save it to xRD+ −
movff xA+0,xRD+0+ −
movff xA+1,xRD+1+ −
; 4. add 160 (display px width)+ −
movlw high(d'160')+ −
addwf xA+1,1+ −
movlw low(d'160')+ −
addwf xA+0,1+ −
btfsc STATUS,C+ −
incf xA+1+ −
; 5. save it to xRDr+ −
movff xA+0,xRDr+0+ −
movff xA+1,xRDr+1+ −
+ −
; ; Bearing ?+ −
; ; We can skip this xRD180 calculation if no bearing is set+ −
; bcf compass_bearing_set+ −
; movff compass_bearing+0,sub_a+0+ −
; movff compass_bearing+1,sub_a+1+ −
; movlw d'0'+ −
; cpfseq sub_a+1+ −
; bra TFT_dive_compass_bearing ; something set, calculate xRD180+ −
; movlw d'0'+ −
; cpfseq sub_a+0+ −
; bra TFT_dive_compass_bearing ; something set, calculate xRD180+ −
; bra TFT_dive_compass_ruler ; no value in the bearing, skip calc+ −
;+ −
;TFT_dive_compass_bearing:+ −
; bsf compass_bearing_set+ −
+ −
btfss compass_bearing_set+ −
bra TFT_dive_compass_ruler ; no value in the bearing, skip calc+ −
+ −
; we have bearing set, we will need xRD180 calculated+ −
; xRD180 is xRDr-180+ −
movff xRDr+1,sub_a+1+ −
movff xRDr+0,sub_a+0+ −
movlw high(d'180')+ −
movff WREG,sub_b+1+ −
movlw low(d'180')+ −
movff WREG,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
movff sub_c+1,xRD180+1+ −
movff sub_c+0,xRD180+0+ −
+ −
TFT_dive_compass_bearing_1:+ −
; calculate bearing position and visibility (ahead or behind)+ −
bcf compass_bearing_vis ; default is not-visibly+ −
bcf compass_bearing_ahd ; default is behind+ −
; get the bearing virtual display offset, store it to divA+ −
movff compass_bearing+0,xA+0+ −
movff compass_bearing+1,xA+1+ −
movlw high(d'360')+ −
addwf xA+1,1+ −
movlw low(d'360')+ −
addwf xA+0,1+ −
btfsc STATUS,C+ −
incf xA+1+ −
; save it to reuse for upper/lower turns and ahead/behind checks+ −
movff xA+1,divA+1+ −
movff xA+0,divA+0+ −
+ −
; check if it's ahead+ −
; load the bearing offset into sub_a+ −
movff divA+1,sub_a+1+ −
movff divA+0,sub_a+0+ −
; load the display offset back to sub_b+ −
movff xRD+0,sub_b+0+ −
movff xRD+1,sub_b+1+ −
rcall TFT_dive_compass_bearing_ap+ −
;test if we found it+ −
btfsc compass_bearing_vis+ −
bra TFT_dive_compass_bearing_dir+ −
+ −
; check if it's ahead with an upper turn+ −
; load the bearing offset into sub_a+ −
movff divA+1,sub_a+1+ −
movff divA+0,sub_a+0+ −
; load the display offset back to sub_b+ −
movff xRD+0,sub_b+0+ −
movff xRD+1,sub_b+1+ −
movlw high(d'360')+ −
addwf sub_b+1,1+ −
movlw low(d'360')+ −
addwf sub_b+0,1+ −
btfsc STATUS,C+ −
incf sub_b+1+ −
rcall TFT_dive_compass_bearing_ap+ −
;test if we found it+ −
btfsc compass_bearing_vis+ −
bra TFT_dive_compass_bearing_dir+ −
+ −
; check if it's ahead with a lower turn+ −
; load the bearing offset into sub_a+ −
movff divA+1,sub_a+1+ −
movff divA+0,sub_a+0+ −
movlw high(d'360')+ −
addwf sub_a+1,1+ −
movlw low(d'360')+ −
addwf sub_a+0,1+ −
btfsc STATUS,C+ −
incf sub_a+1+ −
; load the display offset back to sub_b+ −
movff xRD+0,sub_b+0+ −
movff xRD+1,sub_b+1+ −
rcall TFT_dive_compass_bearing_ap+ −
;test if we found it+ −
btfsc compass_bearing_vis+ −
bra TFT_dive_compass_bearing_dir+ −
+ −
; marker is not ahead of us, check if it's behind us+ −
; use the (160 - (xRD180 - xCM)) formula to see if it's on the display+ −
; load the display offset back to sub_a+ −
movff xRD180+0,sub_a+0+ −
movff xRD180+1,sub_a+1+ −
; load the marker's offset into sub_b+ −
movff divA+0,sub_b+0+ −
movff divA+1,sub_b+1+ −
rcall TFT_dive_compass_bearing_bp+ −
;test if we found it+ −
btfsc compass_bearing_vis+ −
bra TFT_dive_compass_bearing_dir+ −
+ −
;check if it's behind with the lower turn+ −
movff xRD180+0,sub_a+0+ −
movff xRD180+1,sub_a+1+ −
movlw high(d'360')+ −
addwf sub_a+1,1+ −
movlw low(d'360')+ −
addwf sub_a+0,1+ −
btfsc STATUS,C+ −
incf sub_a+1+ −
; load the marker's offset into sub_b+ −
movff divA+0,sub_b+0+ −
movff divA+1,sub_b+1+ −
rcall TFT_dive_compass_bearing_bp+ −
;test if we found it+ −
btfsc compass_bearing_vis+ −
bra TFT_dive_compass_bearing_dir+ −
+ −
; check if it's behind with the upper turn+ −
movff divA+1,sub_b+1+ −
movff divA+0,sub_b+0+ −
movlw high(d'360')+ −
addwf sub_b+1,1+ −
movlw low(d'360')+ −
addwf sub_b+0,1+ −
btfsc STATUS,C+ −
incf sub_b+1+ −
rcall TFT_dive_compass_bearing_bp+ −
bra TFT_dive_compass_bearing_dir+ −
+ −
TFT_dive_compass_bearing_ap:+ −
; xCM received in sub_a+ −
; xRD received in sub_b+ −
; 1/a. check if it's viewable from the left side+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; xRD>divA+ −
return ;no,+ −
; yes, store the RO=RP-RD for drawing+ −
movff sub_c+0,xC+0+ −
movff sub_c+1,xC+1+ −
; 1/b. check if it's viewable from the right side?+ −
movlw d'2' ; avoid thin mess on the side of the display+ −
addwf sub_a+0,1+ −
btfsc STATUS, C+ −
incf sub_a+1+ −
; load the display offset right side into sub_b+ −
movlw high(d'160')+ −
addwf sub_b+1,1+ −
movlw low(d'160')+ −
addwf sub_b+0,1+ −
btfsc STATUS,C+ −
incf sub_b+1+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfss neg_flag ; xRDr>xA(+2)+ −
return ; no,+ −
; print the bearing lines on the screen+ −
movff xC+0,xCM+ −
bsf compass_bearing_vis ; set visible+ −
bsf compass_bearing_ahd ; set ahead+ −
return ; done,+ −
+ −
TFT_dive_compass_bearing_bp:+ −
; use the (160 - (xRD180 - xCM)) formula to see if it's on the display+ −
; the marker's offset received in sub_b+ −
; the xRD180 display offset received in sub_a+ −
; xRD180 - xCM+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; CM>xRD180+ −
return ; no, not on screen+ −
; 160 - (X)+ −
movlw high(d'160')+ −
movff WREG,sub_a+1+ −
movlw low(d'160')+ −
movff WREG,sub_a+0+ −
movff sub_c+1,sub_b+1+ −
movff sub_c+0,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; X>160+ −
return ; no, not on screen+ −
; check if not overflow - this sounds a double check...+ −
movlw d'1'+ −
cpfslt sub_c+1+ −
return ; high set, >160+ −
movlw d'160'+ −
cpfslt sub_c+0+ −
return ; low >160+ −
; print the bearing lines on the screen+ −
movff sub_c+0,xCM+ −
bsf compass_bearing_vis+ −
return ; done+ −
+ −
TFT_dive_compass_bearing_dir:+ −
; check if bearing to heading, and calculate the direction+ −
bcf compass_bearing_eq+ −
btfss compass_bearing_vis+ −
bra TFT_dive_compass_bearing_lr+ −
btfss compass_bearing_ahd+ −
bra TFT_dive_compass_bearing_lr+ −
movff xCM,xA+0+ −
movlw d'80'+ −
cpfseq xA+0+ −
bra TFT_dive_compass_bearing_lr+ −
bsf compass_bearing_eq+ −
bra TFT_dive_compass_ruler ; bearing points to heading, no signs are required, go to the ruler+ −
+ −
TFT_dive_compass_bearing_lr:+ −
; get the bearing virtual display offset+ −
movff compass_bearing+0,xA+0+ −
movff compass_bearing+1,xA+1+ −
; divA =IF (U10>292;U10;U10+360)+ −
movlw high(d'292')+ −
movff WREG,sub_a+1+ −
movlw low(d'292')+ −
movff WREG,sub_a+0+ −
movff xA+1,sub_b+1+ −
movff xA+0,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; xA>292+ −
bra TFT_dive_compass_bearing_lr_1 ;yes+ −
; no, xA<=292+ −
movlw high(d'360')+ −
addwf xA+1,1+ −
movlw low(d'360')+ −
addwf xA+0,1+ −
btfsc STATUS,C+ −
incf xA+1+ −
TFT_dive_compass_bearing_lr_1:+ −
; 1. calculate whether bearing is to left or to right+ −
bsf compass_bearing_lft ; to the left by default+ −
; xC: save center value to compare the direction to front value+ −
movff xA+1,xC+1+ −
movff xA+0,xC+0+ −
; xB: we need the left side for comparism... left = -180+ −
movff xA+1,sub_a+1+ −
movff xA+0,sub_a+0+ −
movlw high(d'180')+ −
movff WREG,sub_b+1+ −
movlw low(d'180')+ −
movff WREG,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
movff sub_c+1,xB+1 ; xB has the left side of the 180° distance center+ −
movff sub_c+0,xB+0+ −
; xA = IF(xRD>(xC+100);xRD-280;xRD+80)+ −
movff xC+1,sub_a+1+ −
movff xC+0,sub_a+0+ −
movlw d'100'+ −
addwf sub_a+0,1+ −
btfsc STATUS,C+ −
incf sub_a+1+ −
movff xRD+1,sub_b+1+ −
movff xRD+0,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; xRD>xC+100+ −
bra TFT_dive_compass_bearing_lr_2 ; yes, xA=xRD-280+ −
; no, xA = xRD+80+ −
movff xRD+1,xA+1+ −
movff xRD+0,xA+0+ −
movlw d'80'+ −
addwf xA+0,1+ −
btfsc STATUS,C+ −
incf xA+1+ −
bra TFT_dive_compass_bearing_lr_c+ −
+ −
TFT_dive_compass_bearing_lr_2:+ −
; xA=xRD-280+ −
movff xRD+1,sub_a+1+ −
movff xRD+0,sub_a+0+ −
movlw high(d'280')+ −
movff WREG,sub_b+1+ −
movlw low(d'280')+ −
movff WREG,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
movff sub_c+1,xA+1+ −
movff sub_c+0,xA+0+ −
;bra TFT_dive_compass_bearing_lr_c+ −
+ −
TFT_dive_compass_bearing_lr_c:+ −
; xB < xA < xC => right, otherwise left (default)+ −
movff xA+1,sub_b+1+ −
movff xA+0,sub_b+0+ −
movff xB+1,sub_a+1+ −
movff xB+0,sub_a+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfss neg_flag ; xA>xB ?+ −
bra TFT_dive_compass_ruler ; No, xB >= xA, keep default left+ −
movff xA+1,sub_a+1+ −
movff xA+0,sub_a+0+ −
movff xC+1,sub_b+1+ −
movff xC+0,sub_b+0+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfss neg_flag ; xC>xA ?+ −
bra TFT_dive_compass_ruler ; No, xA >= xC, keep default left+ −
bcf compass_bearing_lft+ −
+ −
TFT_dive_compass_ruler:+ −
; calculate mod15 for the ticks+ −
movff xRD+0,xA+0+ −
movff xRD+1,xA+1+ −
movlw d'15'+ −
movwf xB+0+ −
clrf xB+1+ −
call div16x16 ;xA/xB=xC with xA+0 as remainder+ −
; check xA+0, it has the remainder+ −
movlw d'0'+ −
cpfsgt xA+0 ; mod15 > 0+ −
bra TFT_dive_compass_ruler_1 ; no, RM = 0+ −
; yes RM = 15 - RDmod15+ −
movlw d'15'+ −
subfwb xA+0,1+ −
TFT_dive_compass_ruler_1:+ −
; xA+0 holds the RM, store it to 'lo'+ −
movff xA+0,lo+ −
; init DD to zero, store it to 'hi'+ −
movlw d'0'+ −
movff WREG,hi+ −
+ −
TFT_dive_compass_ruler_loop:+ −
; 1. check if we run of from the display+ −
movlw d'160' ; Looks like 160 works because TFT_box limits the dispay+ −
cpfslt lo,1+ −
bra TFT_dive_compass_ruler_lend ; xRM >= W+ −
; 2. Clear the tick area from DD to RM - in segments to avoid blinking+ −
; don't do a clear if we are at 0 (zero) otherwise it will blink+ −
; because of the width underflow+ −
movlw d'0'+ −
cpfsgt lo,1+ −
bra TFT_dive_compass_ruler_loop_zz+ −
rcall TFT_dive_compass_clr_ruler+ −
TFT_dive_compass_ruler_loop_zz:+ −
; 3. Draw the markers @ RM+ −
rcall TFT_dive_compass_ruler_print+ −
; 4. If D<82 and RM>79: means we put something over the center line+ −
; redraw the center line+ −
movlw d'82'+ −
cpfslt hi,1+ −
bra TFT_dive_compass_ruler_loop_zz2+ −
movlw d'79'+ −
cpfsgt lo,1+ −
bra TFT_dive_compass_ruler_loop_zz2+ −
rcall TFT_dive_compass_cline ;enough to print cline as bearing marker is not in the ticker area+ −
TFT_dive_compass_ruler_loop_zz2:+ −
; 5. set D = RM + 2 : position after the 2px tick+ −
movff lo,hi+ −
movlw d'2'+ −
addwf hi,1+ −
; 6. set RM = RM + 15 : position to the next tick+ −
movlw d'15'+ −
addwf lo,1+ −
; 7. loop+ −
bra TFT_dive_compass_ruler_loop+ −
+ −
TFT_dive_compass_ruler_lend: ; loop end+ −
; 8. clear the rest of the tick area if D<160+ −
movlw d'160'+ −
cpfslt hi,1+ −
bra TFT_dive_compass_ruler_lend2 ; D >= W+ −
; 9. position left to end of display to clear the remaining area+ −
movlw d'160'+ −
movwf lo+ −
; 10. clear it+ −
rcall TFT_dive_compass_clr_ruler+ −
+ −
TFT_dive_compass_ruler_lend2:+ −
; done with the compass ruler, put the labels on the screen+ −
; get the RD abck to sub_b+ −
movff xRD+0,sub_b+0+ −
movff xRD+1,sub_b+1+ −
; hi stores the display position+ −
movlw d'0'+ −
movwf hi+ −
; lo stores the last item's display position+ −
movlw d'0'+ −
movwf lo+ −
bcf print_compass_label+ −
+ −
movlw d'14'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'219' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'219' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_1+ −
STRCPY_TEXT_PRINT tSW ; yes - print it+ −
dcr_1:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'7'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'267' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'267' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_2+ −
STRCPY_TEXT_PRINT tW ; yes - print it+ −
dcr_2:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'14'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'309' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'309' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_3+ −
STRCPY_TEXT_PRINT tNW ; yes - print it+ −
dcr_3:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'6'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'358' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'358' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_4+ −
STRCPY_TEXT_PRINT tN ; yes - print it+ −
dcr_4:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'13'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'399' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'399' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_5+ −
STRCPY_TEXT_PRINT tNE ; yes - print it+ −
dcr_5:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'6'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'448' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'448' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_6+ −
STRCPY_TEXT_PRINT tE ; yes - print it+ −
dcr_6:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'13'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'489' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'489' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_7+ −
STRCPY_TEXT_PRINT tSE ; yes - print it+ −
dcr_7:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'6'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'538' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'538' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_8+ −
STRCPY_TEXT_PRINT tS ; yes - print it+ −
dcr_8:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'14'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'579' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'579' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_9+ −
STRCPY_TEXT_PRINT tSW ; yes - print it+ −
dcr_9:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'7'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'627' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'627' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_10+ −
STRCPY_TEXT_PRINT tW ; yes - print it+ −
dcr_10:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'14'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'669' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'669' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_11+ −
STRCPY_TEXT_PRINT tNW ; yes - print it+ −
dcr_11:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
movlw d'6'+ −
movwf up ; up stores the width of hte label+ −
movlw low( d'718' ) ; position of the label+ −
movwf sub_a+0+ −
movlw high( d'718' )+ −
movwf sub_a+1+ −
rcall TFT_dive_compass_label_proc ; check if the label should be on screen+ −
btfss print_compass_label ; Yes?+ −
bra dcr_12+ −
STRCPY_TEXT_PRINT tN ; yes - print it+ −
dcr_12:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
+ −
TFT_dive_compass_label_end:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
; restore lo and hi for the final cleanup+ −
movff xLO,lo+ −
movff xHI,hi+ −
; clear the rest of the SQ area if there are more space+ −
movlw d'160'+ −
cpfslt hi+ −
bra TFT_dive_compass_label_end2 ; D >= 160, no more space+ −
; position left to end of display to clear the remaining area+ −
movlw d'160'+ −
movff WREG,lo+ −
; clear it+ −
rcall TFT_dive_compass_clr_label+ −
TFT_dive_compass_label_end2:+ −
rcall TFT_dive_compass_c_mk ; check if label is on the center line or the marker+ −
; do we have bearing set?+ −
btfsc compass_bearing_set+ −
bra TFT_dive_compass_dir_text ; bearing_set=1 - go and print the dir (<< or >>)+ −
rcall TFT_dive_compass_dir_lclr ; no, clear the area (e.g. we had but removed)+ −
rcall TFT_dive_compass_dir_rclr+ −
bra TFT_dive_compass_text+ −
+ −
TFT_dive_compass_dir_text:+ −
; bearing set, but does it point to heading?+ −
btfss compass_bearing_eq+ −
bra TFT_dive_compass_dir_text_2 ; bearing != heading - go and print the dir+ −
rcall TFT_dive_compass_dir_lclr ; bearing = heading, no need for direction markers+ −
rcall TFT_dive_compass_dir_rclr+ −
bra TFT_dive_compass_text+ −
+ −
TFT_dive_compass_dir_text_2:+ −
movlw color_green+ −
call TFT_set_color+ −
btfsc compass_bearing_lft+ −
bra TFT_dive_compass_dir_ldir ; bearing_lft=1, print the left marker+ −
;TFT_dive_compass_text_rdir:+ −
WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row+ −
STRCPY_PRINT ">>"+ −
; do not forget to clear the left+ −
rcall TFT_dive_compass_dir_lclr+ −
bra TFT_dive_compass_text+ −
+ −
TFT_dive_compass_dir_ldir:+ −
WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row+ −
STRCPY_PRINT "<<"+ −
; do not forget to clear the right+ −
rcall TFT_dive_compass_dir_rclr+ −
;bra TFT_dive_compass_text+ −
+ −
TFT_dive_compass_text:+ −
; Text output+ −
call TFT_standard_color+ −
WIN_SMALL dm_custom_compass_head_column, dm_custom_compass_head_row+ −
rcall TFT_surface_compass_heading_com ; Show "000° N"+ −
return+ −
+ −
TFT_dive_compass_dir_lclr:+ −
WIN_SMALL dm_custom_compass_ldir_column, dm_custom_compass_head_row+ −
STRCPY_PRINT " "+ −
return+ −
+ −
TFT_dive_compass_dir_rclr:+ −
WIN_SMALL dm_custom_compass_rdir_column, dm_custom_compass_head_row+ −
STRCPY_PRINT " "+ −
return+ −
+ −
TFT_dive_compass_label_proc:+ −
; Input:+ −
; xHI: DD - display'a current position+ −
; xRD: RD - ruler display offset+ −
; sub_a: RP - item's ruler display offset+ −
; get the RD abck to sub_b+ −
movff xHI,hi+ −
bcf print_compass_label+ −
; 1/a. check if it's viewable ? sub_a(RP) >= sub_b(RD) ?+ −
; set the carry flag if sub_b(xRD) is equal to or greater than sub_a(xRP):+ −
movff xRD+0,sub_b+0+ −
movff xRD+1,sub_b+1+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfsc neg_flag ; >=0?+ −
return ; No+ −
; store the RO=RP-RD for drawing+ −
movff sub_c+0,xC+0+ −
movff sub_c+1,xC+1+ −
+ −
; 1/b. check if it's viewable ? sub_a(RP)+up(width) < sub_b(RD)+160+ −
; if already above, no need to process the rest of the labels+ −
;movff up,WREG ; don't worry about the width, low level call prevents overload+ −
movlw d'2' ; .. but still avoid thin mess on the side of the display+ −
addwf sub_a+0,1+ −
btfsc STATUS, C+ −
incf sub_a+1+ −
+ −
movff xRDr+0,sub_b+0+ −
movff xRDr+1,sub_b+1+ −
call subU16 ; sub_c = sub_a - sub_b+ −
btfss neg_flag ; ? <0+ −
bra TFT_dive_compass_label_end ; No+ −
+ −
; 2. restore RO=RP-RD from 1/a.+ −
movff xC+0,lo+ −
+ −
; 3. Clear the segment from DD(hi) to lo+ −
; don't do a clear if we are at 0 (zero) otherwise it will blink+ −
; ?because of the width underflow?+ −
movlw d'0'+ −
cpfsgt lo+ −
bra TFT_dive_compass_label_proc_p+ −
rcall TFT_dive_compass_clr_label+ −
TFT_dive_compass_label_proc_p:+ −
; 4. print the SQ on the screen+ −
call TFT_standard_color+ −
bsf print_compass_label+ −
;TFT_dive_compass_label_print:+ −
movlw dm_custom_compass_label_row+ −
movff WREG,win_top+ −
movff lo,win_leftx2+ −
movlw FT_SMALL+ −
movff WREG,win_font+ −
; 6. retain the new display positions+ −
movff hi,divB ; old-hi will be used by the c_mk : clear+marker printing+ −
movff lo,hi+ −
movff up,WREG+ −
addwf hi,1+ −
movff lo,xLO+ −
movff hi,xHI+ −
return+ −
+ −
TFT_dive_compass_c_mk:+ −
; Common task to draw center line and marker+ −
; until a proper implementation make it simple:+ −
rcall TFT_dive_compass_mk+ −
rcall TFT_dive_compass_cline+ −
return+ −
+ −
TFT_dive_compass_mk:+ −
; draw the bearing on the screen if visible and if we just put something over it+ −
btfss compass_bearing_set+ −
return ; bearing_set=0 nothing to display+ −
+ −
btfss compass_bearing_vis+ −
return ; bearing set but not visible+ −
+ −
; save lo/hi from trashing+ −
movff lo,xA+0+ −
movff hi,xA+1+ −
+ −
; did we just update the marker's position?+ −
; DD.......DD+ −
; CM+2>=DD(old) or CM-2<=DD+ −
; ToDo+ −
+ −
btfss compass_bearing_ahd+ −
bra TFT_dive_compass_mk_rear+ −
;TFT_dive_compass_mk_front:+ −
clrf lo+ −
movff xCM,lo+ −
bsf print_compass_label ; set=green marker+ −
rcall TFT_dive_compass_mk_print+ −
bcf print_compass_label+ −
bra TFT_dive_compass_mk_end+ −
+ −
TFT_dive_compass_mk_rear:+ −
clrf lo+ −
movff xCM,lo+ −
bcf print_compass_label ; set=red marker+ −
rcall TFT_dive_compass_mk_print+ −
+ −
TFT_dive_compass_mk_end:+ −
movff xA+0,lo+ −
movff xA+1,hi+ −
return+ −
+ −
TFT_dive_compass_mk_print:+ −
movlw d'1'+ −
cpfsgt lo+ −
bra TFT_dive_compass_mk_print_2 ; lo<=1, skip the first line+ −
movlw d'2'+ −
subwf lo,0+ −
; movff WREG,win_leftx2+ −
rcall TFT_dive_compass_mk_print_3+ −
TFT_dive_compass_mk_print_2:+ −
; save hi/lo+ −
movff hi,divA+1+ −
movff lo,divA+0+ −
; clear the middle of hte bearing marker+ −
movff lo,hi+ −
movlw d'2'+ −
addwf lo,1+ −
rcall TFT_dive_compass_clr_label+ −
; restore hi/lo+ −
movff divA+1,hi+ −
movff divA+0,lo+ −
; print a dot on the middle+ −
movff lo,WREG+ −
rcall TFT_dive_compass_mk_print_dot+ −
; finally print the right marker line+ −
movlw d'2'+ −
addwf lo,0+ −
; rcall TFT_dive_compass_mk_print_3+ −
; return+ −
TFT_dive_compass_mk_print_3:+ −
movff WREG,win_leftx2+ −
movlw dm_custom_compass_label_row+ −
movff WREG,win_top+ −
movlw dm_custom_compass_label_height-.2+ −
movff WREG,win_height+ −
bra TFT_dive_compass_mk_print_4+ −
TFT_dive_compass_mk_print_dot:+ −
movff WREG,win_leftx2+ −
movlw dm_custom_compass_label_row + .9+ −
movff WREG,win_top+ −
movlw d'4'+ −
movff WREG,win_height+ −
TFT_dive_compass_mk_print_4:+ −
movlw d'2'+ −
movff WREG,win_width+ −
movff WREG,win_bargraph+ −
movlw color_green+ −
btfss print_compass_label+ −
movlw color_red+ −
call TFT_set_color+ −
call TFT_box+ −
return+ −
+ −
TFT_dive_compass_clr_label:+ −
movlw dm_custom_compass_label_row-.2 ; set top & height+ −
movff WREG,win_top+ −
movlw dm_custom_compass_label_height+.2+ −
movff WREG,win_height+ −
rcall TFT_dive_compass_clear+ −
return+ −
+ −
TFT_dive_compass_clr_ruler:+ −
; top tick+ −
movlw dm_custom_compass_tick_top_top ; set top & height+ −
movff WREG,win_top+ −
movlw dm_custom_compass_tick_height+ −
movff WREG,win_height+ −
rcall TFT_dive_compass_clear+ −
;bottom tick+ −
movlw dm_custom_compass_tick_bot_top ; set top & height+ −
movff WREG,win_top+ −
movlw dm_custom_compass_tick_height+ −
movff WREG,win_height+ −
; rcall TFT_dive_compass_clear+ −
; return+ −
TFT_dive_compass_clear:+ −
; we receive RM in lo and DD in hi+ −
; calculate width = RM-D+ −
movff hi,WREG+ −
subwf lo,0+ −
movff WREG,win_width ; RM-DD+ −
movff WREG,win_bargraph+ −
; incf hi,W ; +1 pixel to avopid clipping of chars+ −
; movff WREG,win_leftx2+ −
movff hi,win_leftx2+ −
movlw color_black+ −
call TFT_set_color+ −
call TFT_box+ −
return+ −
+ −
TFT_dive_compass_ruler_print:+ −
; we receive RM in lo and DD in hi+ −
movlw dm_custom_compass_tick_top_top+ −
movff WREG,win_top+ −
movlw dm_custom_compass_tick_height+ −
movff WREG,win_height+ −
movlw d'2'+ −
movff WREG,win_width+ −
movff WREG,win_bargraph+ −
movff lo,win_leftx2 ; 0..159+ −
call TFT_standard_color+ −
call TFT_box+ −
movlw dm_custom_compass_tick_bot_top+ −
movff WREG,win_top+ −
movlw dm_custom_compass_tick_height+ −
movff WREG,win_height+ −
call TFT_standard_color ; color in WREG is trashed, must be set again!+ −
call TFT_box+ −
return+ −
+ −
TFT_dive_compass_cline:+ −
movlw color_yellow+ −
WIN_BOX_COLOR dm_custom_compass_tick_top_top, dm_custom_compass_tick_bot_bot,.80,.81+ −
return+ −
+ −
tft_compass_cardinal:+ −
btfsc hi,0 ; Heading >255°?+ −
bra tft_compass_cardinal2 ; Yes must be W, NW or N+ −
; No, Must be W, SW, S, SE, E, NE or N+ −
movlw .23+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_N+ −
movlw .68+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_NE+ −
movlw .113+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_E+ −
movlw .158+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_SE+ −
movlw .203+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_S+ −
movlw .248+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_SW+ −
bra tft_compass_cardinal_W+ −
+ −
tft_compass_cardinal2:+ −
movlw .37+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_W+ −
movlw .82+ −
subwf lo,W+ −
btfss STATUS,C+ −
bra tft_compass_cardinal_NW+ −
; bra tft_compass_cardinal_N+ −
tft_compass_cardinal_N:+ −
STRCAT_TEXT tN+ −
return+ −
tft_compass_cardinal_NE:+ −
STRCAT_TEXT tNE+ −
return+ −
tft_compass_cardinal_E:+ −
STRCAT_TEXT tE+ −
return+ −
tft_compass_cardinal_SE:+ −
STRCAT_TEXT tSE+ −
return+ −
tft_compass_cardinal_S:+ −
STRCAT_TEXT tS+ −
return+ −
tft_compass_cardinal_SW:+ −
STRCAT_TEXT tSW+ −
return+ −
tft_compass_cardinal_W:+ −
STRCAT_TEXT tW+ −
return+ −
tft_compass_cardinal_NW:+ −
STRCAT_TEXT tNW+ −
return+ −
+ −
compass_heading_common:+ −
call speed_normal+ −
movlw compass_averaging ; numbers of extra averaging+ −
movwf up+ −
compass_heading_common2:+ −
rcall TFT_get_compass+ −
decfsz up,F+ −
bra compass_heading_common2+ −
extern compass+ −
call compass ; Do compass corrections.+ −
banksel common+ −
+ −
; More then compass_fast_treshold?+ −
movff compass_heading_old+0,sub_a+0+ −
movff compass_heading_old+1,sub_a+1+ −
movff compass_heading+0,sub_b+0+ −
movff compass_heading+1,sub_b+1+ −
call sub16+ −
btfss neg_flag ; <0?+ −
bra compass_heading_common3 ; No, test for threshold+ −
; Yes, subtract the other way round+ −
movff compass_heading+0,sub_a+0+ −
movff compass_heading+1,sub_a+1+ −
movff compass_heading_old+0,sub_b+0+ −
movff compass_heading_old+1,sub_b+1+ −
call sub16+ −
compass_heading_common3:+ −
movff compass_heading+0,compass_heading_old+0 ; copy new "old"+ −
movff compass_heading+1,compass_heading_old+1+ −
+ −
bcf compass_fast_mode+ −
movlw compass_fast_treshold+ −
cpfslt sub_c+0 ; > compass_fast_treshold?+ −
bsf compass_fast_mode ; Yes!+ −
+ −
btfss compass_fast_mode ; In fast mode?+ −
return ; No.+ −
; Yes.+ −
movff compass_heading+0,lo+ −
movff compass_heading+1,hi+ −
call TFT_convert_signed_16bit ; converts lo:hi into signed-short and adds '-' to POSTINC2 if required+ −
movff lo,compass_heading_shown+0+ −
movff hi,compass_heading_shown+1+ −
return+ −
+ −
TFT_get_compass:+ −
call I2C_RX_compass ; Test Compass+ −
call I2C_RX_accelerometer ; Test Accelerometer+ −
call compass_filter ; Filter Raw compass + accel readings.+ −
banksel common+ −
return+ −
+ −
+ −
END+ −