;we now have 8 ranges of 45 degrees each to sort out
;x_input and y_input are in 2s compliment
clrf angle_range
btfss y_input, 7 ;see if y is negative
goto north_half
bsf angle_range, 0 ;this means that the bearing is in the south half
comf y_input, f
incf y_input, f ;negate if needed so that y is now positive
north_half
btfss x_input, 7
goto quadrants_done
bsf angle_range, 1 ;this mans that the bearing is in the west half
comf x_input, f
incf x_input, f ;negate if needed so that x is now positive
quadrants_done
clrf bottom_byte
movlw 0xff
movwf count
movf y_input, w
subwf x_input, w
btfsc STATUS, C
goto no_xy_swap ;compare longditude difference with latitude difference
bsf angle_range, 2 ;note if needed and swap the registers
;this means that the y was larger, which happens if the
;bearing is nearer to North or South than it is to East or West
movf y_input, w
subwf x_input, w
addwf y_input, f
subwf x_input, f ;swap x and y differences
no_xy_swap
;x is now largest
;This divides the y * 256 by the x, optimised for code length
divide_loop
incf count, f ;count the number of subtractions
incf count, w
btfsc STATUS, Z
goto divide_done ;skip out if the divide is taking too long
movf x_input, w
subwf bottom_byte, f
btfss STATUS, C
decf y_input, f ;subtract 1 bytes from 2 bytes
btfss y_input, 7
goto divide_loop ;this keeps the routine returning to here until divide is finished
divide_done
;count is now 0 - 0xff representing tan(angle) in the range 0 - 45 deg
;there is a lookup table for the values, 0x00, 0x40, 0x80, 0xC0 and 0x100
;the value looked up is 4 times the tan in degrees
;the values either side of the current tan are looked up
;the last 6 bits are multiplied by the difference in the lookup table
;we want the atan of the number /256
clrf count1
bcf STATUS, C
rlf count, f
rlf count1, f
rlf count, f ;collect the top 2 bits and
rlf count1, f ;multiply what is left by 4, because it will later be divided by 4
incf count1, f
call arc_tan_table ;Get a2=atan( (x>>6) + 1)
movwf count2 ;Store temporarily in count2
decf count1, f ;Get the saved index
call arc_tan_table ;Get a1=atan( (x>>6) )
subwf count2, w ;W=a2-a1, This is always positive.
subwf count2, f ;a1 = a1 - (a1-W) = W ;now count2 is the lower value and w is the difference
;w needs to be multiplied by count
clrf multh
; clrf multl ;not needed but can be left in to make the multiply easier to understand
clrf mult_count
bsf mult_count, 3 ;set to 8 without using w
mult_loop
bcf STATUS, C
btfsc count, 0
addwf multh, f ;add if needed
rrf multh, f ;rotate result
; rrf multl, f ;not needed but can be left in to make the multiply easier to understand
rrf count, f ;rotate count
decfsz mult_count, f
goto mult_loop ;the difference is multiplied,
movf multh, w ;take the high byte
addwf count2, f
bcf STATUS, C
rrf count2, f
bcf STATUS, C
rrf count2, w
movwf count ;divide by 4
;now count is the angle in degrees from 0 - 45
;angle_range says which quadrant and which half the angle is in
;0 45 - 90 0x82
;1 90 - 135 0x02
;2 270 - 315 0x04
;3 225 - 270 0x84
;4 0 - 45 0x01
;5 135 - 180 0x83
;6 315 - 360 0x85
;7 180 - 225 0x03
;The lookup table ends with the last 3 bits representing how many lots of 90 degrees the start angle is, +1
;and bit 7 is set if the value in count should be taken as negative
call angle_range_table
movwf angle_range
btfss angle_range, 7
goto angle_increment_positive
comf count, f
incf count, f ;negate if required
angle_increment_positive
movlw d'256'-d'45'
movwf count1 ;start value set so that after one addition the result is zero
bcf angle_range, 7
movlw d'45'
next45
addwf count1, f
decfsz angle_range, f
goto next45 ;add 45 the correct number of times
bcf STATUS, C
clrf count2
rlf count1, f ;multiply by 2
rlf count2, f ;now 0, 90, 180, 270 or 360
movf count, w
addwf count1, f
btfsc STATUS, C
incf count2, f
btfsc count, 7
decf count2, f ;add the difference, dealing with carry and negatives as required
btfss count2, 0
goto no_roll
movlw d'360'-d'256'
subwf count1, w
btfss STATUS, C
goto no_roll
movwf count1
clrf count2 ;mod 360 in case nothing is subtracted from 360
no_roll
;and that's it. The angle in degrees is in count2:count1
arc_tan_table
btfsc count1, 2
retlw d'182'
btfsc count1, 1
goto top_half_atan_table
btfss count1, 0
retlw d'3'
retlw d'59'
top_half_atan_table
btfss count1, 0
retlw d'110'
retlw d'150'
angle_range_table
movlw high(angle_range_lookup_start)
movwf PCLATH
angle_range_lookup
movf angle_range, w
andlw b'00000111'
addwf PCL, f
angle_range_lookup_start
retlw 0x82
retlw 0x02
retlw 0x04
retlw 0x84
retlw 0x01
retlw 0x83
retlw 0x85
retlw 0x03