Efficient Fixed-Point Trigonometry For PIC16F

[ScaleRobotics]

Microchip has an application note AN1061 http://ww1.microchip.com/downloads/en/AppNotes/01061A.pdf which talks about using the ATAN function, among other things. I can't wait to use it for a project I am working on ........ I just need to know how. .....

I am a newbie when it comes to adding includes. I tried a few different ways to add the assembly code for the atan, and I could not get it to compile. I tried using the math16.asm file as an include in a new main program, I also tried cutting and pasting the assembly into a main.pbp program with asm and endasm lines on the front and back. Can someone steer me in the right direction? This cordic function would be perfect for me.

I get the following error when trying to compile with MPASM using MicroCode Studio Plus: Error[149] Directive only allowed when generating an object file.
I get this for multiple line listings of the assembly code, but this does not seem to make sense for the lines it references. Like

movlw 0x00 ; Y_H & Y_L is loaded with 0x0000

Thanks for your help.

Walter

[Darrel Taylor]

Hi Walter,

The MATH16.ASM file looks interesting.
It's written as Relocatable Object Code, so isn't directly compatible with PBP.

But with a bit of work you could modify it.

Statements like UDATA, CODE , GLOBAL , and END get removed, or commented.

Names with RES are variables, so they need to be defined in Basic.
Something like ...
X VAR WORD
@X_H = _X + 1
@X_L = _X

And some of the labels need to be renamed.
DIV and MUL already exist is PBP.

[ScaleRobotics]

Thanks for the pointers. I was able to get ATAN working! Wooohooo! I must have done something to the sincos function though. I get the same result, no matter what variable I enter into x. It is supposed to cos the value of x and give the result in y, and sin the value of x, and give the result in z. I'm a little brain dead right now, so I will have to check it further in the morning.



;************************************************* ***************************
;* Microchip Technology Inc. 2006 *
;* Filename: Main.asm *
;* *
;************************************************* ***************************

DEFINE __16F877A 1 ; list directive to define processor
DEFINE OSC 20 ' Define Oscillator Speed
DEFINE LOADER_USED 1

' Shutdown comparators
ADCON1 = 7 ; Turn of all A/D
CMCON = 7
' Set the port directions
' 0=output 1=input
TRISA=%00000011 ' Set PORTA 0-1-3 used for AD
TRISB=%11011100 ' Set PortB
TRISC=%10000000
TRISD=%00000000 ' Set PortC USART RX as an input
porta.2 = 0
porta.3 = 0

X_H VAR byte
ITERATION VAR byte
X_L VAR byte
Y_H VAR byte
Y_L VAR byte
Z_H VAR byte
Z_L VAR byte
X_TMP_H VAR byte
X_TMP_L VAR byte
Y_TMP_H VAR byte
Y_TMP_L VAR byte
ITERAT_TMP VAR byte
Di VAR byte
COSCH VAR byte
COSCL VAR byte
Counter1 VAR byte
SHIFT VAR byte
x var word
y var word
z var word
i var word

;---------[change these to match your hardware]------------------------------
DEFINE LCD_DREG PORTD ; Set LCD Data port B C D
DEFINE LCD_DBIT 0 ; Set starting Data bit (0 or 4) 4 0
DEFINE LCD_RSREG PORTA ; Set LCD Register Select port A A
DEFINE LCD_RSBIT 3 ; Set LCD Register Select bit 3 2
DEFINE LCD_EREG PORTA ; Set LCD Enable port A A
DEFINE LCD_EBIT 1 ; Set LCD Enable bit 1 5
DEFINE LCD_BITS 4 ; Set LCD bus size (4 or 8 bits) 4 4
DEFINE LCD_LINES 2 ; Set number of lines on LCD 2 2
'----------------------------------------------------------------------------
clear
lcdout $FE,1
portb.0 = 0
pause 500
lcdout $FE,1,"Test"
portb.0 = 1

Start:
x = 250
y = 0

X_L = x.byte0
X_H = x.byte1

Y_L = y.byte0 'load values of y into Y_L ,just for atan, but does not hurt to leave
Y_H = y.byte1 'dito

call sincos
y.byte0 = Y_L ' edit out if looking at atan
y.byte1 = Y_H 'same here

'Call atan


z.byte0 = Z_L
z.byte1 = Z_H
'z=(z*100)/284 'corrects our output to an angle in degrees for atan
'lcdout $FE,1,#z



portb.0 = 0
lcdout $FE,1,#y
lcdout $FE,$C0,#z
end '***********************************

atan:
asm
call ATAN
endasm
return

sincos:
asm
call SINCOS
endasm
return

asm
;_________________________________________________ _________________________
FSR_ptr macro addr
banksel addr
bcf STATUS,IRP
btfsc STATUS,RP1
bsf STATUS,IRP
movlw LOW addr
movwf FSR
endm

#define Dir _Di,0
#define RV _Di,1
#define _C STATUS,0
#define _Z STATUS,2

Z_stepH_tbl
addwf PCL,f
dt 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
Z_stepL_tbl
addwf PCL,f
dt 0x80,0x4C,0x28,0x14,0x0A,0x05,0x03,0x01
Z_NstepH_tbl
addwf PCL,f
dt 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
Z_NstepL_tbl
addwf PCL,f
dt 0x80,0xB4,0xD8,0xEC,0xF6,0xFB,0xFD,0xFF

ATAN
banksel _Z_H ; Z_H & Z_L is loaded with 0x0000
movlw 0x00 ; Y & X is the input
movwf _Z_H ; X(i+1) = Xi - Yi*_Di*2^-i
movlw 0x00 ; Y(I+1) = Yi + Xi*_Di*2^-i
movwf _Z_L ; Z(i+1) = Zi - _Di*ATAN(2^-i)
banksel _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0
bsf RV ; Set Vectoring Mode
goto CORDIC_Alg

SINCOS
banksel _Y_L ; Z_H & Z_L is the input
movlw 0x00 ; Y_H & Y_L is loaded with 0x0000
movwf _Y_L ; X_H & X_L is loaded with 6070 (1/An constant)
movlw 0x00
movwf _Y_H
movlw LOW .6070
movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i
movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i
movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i)
banksel _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1
bcf RV ; Set Rotational Mode

CORDIC_Alg
banksel _ITERATION
clrf _ITERATION
Rotation_loop
BUFF_X_Y ; Buffers both X & Y into temporary registers
banksel _Y_L
movf _Y_L, W ; Move Y --> T_TEMP
movwf _Y_TMP_L
movf _Y_H, W
movwf _Y_TMP_H
movf _X_L, W ; Move Y --> T_TEMP
movwf _X_TMP_L
movf _X_H, W
movwf _X_TMP_H

banksel _Di ; Vectoring or Rotational Mode?
bcf Dir
btfsc RV
goto Rot
Vec
btfss _Z_H, 7 ; Determine the state of _Di (1 OR -1)
bsf Dir ; _Di=1 when Z_H is pos
goto Cordic
Rot
btfsc _Y_H, 7 ; Determine the state of _Di (1 OR -1)
bsf Dir ; _Di=1 when Y_H is neg

Cordic
movfw _ITERATION
movwf _SHIFT ; Load number of shifts
bsf _SHIFT,7 ; Shift right (Division)
FSR_ptr _Y_TMP_L ; Load FSR
call Shifter ; Execute shift

btfsc Dir
call COM_YTEMP ; Get the 2's complement of teh TEMP register
SUM_X ; Calculate X(i+1)
movf _Y_TMP_H, W
addwf _X_H, F
movf _Y_TMP_L, W
addwf _X_L, F
btfsc _C
incf _X_H, F

movfw _ITERATION
movwf _SHIFT ; Load number of shifts
bsf _SHIFT,7 ; Shift right (Division)
FSR_ptr _X_TMP_L ; Load FSR
call Shifter ; Execute shift

btfss Dir
call COM_XTEMP ; Get the 2's complement of teh TEMP register
SUM_Y ; Calulate Y(i+1)
movf _X_TMP_H, W
addwf _Y_H, F
movf _X_TMP_L, W
addwf _Y_L, F
btfsc _C
incf _Y_H, F

btfss Dir
goto ADD_Z

movlw HIGH Z_NstepH_tbl
movwf PCLATH
movfw _ITERATION ; Fetch appropriate value from table
call Z_NstepH_tbl
addwf _Z_H, F
movlw HIGH Z_NstepL_tbl
movwf PCLATH
movfw _ITERATION
call Z_NstepL_tbl
addwf _Z_L, F
btfsc _C
incf _Z_H, F
goto Rot_Loop_out

ADD_Z
movlw HIGH Z_stepL_tbl
movwf PCLATH
movfw _ITERATION
call Z_stepH_tbl
addwf _Z_H, F
movlw HIGH Z_stepH_tbl
movwf PCLATH
movfw _ITERATION
call Z_stepL_tbl
addwf _Z_L, F
btfsc _C
incf _Z_H, F

Rot_Loop_out
incf _ITERATION,f
btfsc _ITERATION,3
return
goto Rotation_loop

COM_YTEMP
comf _Y_TMP_L, F ; Complememt Y_TMP
comf _Y_TMP_H, F
movlw 01H
addwf _Y_TMP_L, F
btfsc _C
incf _Y_TMP_H, F
return
COM_XTEMP
comf _X_TMP_L, F ; Complememt X_TMP
comf _X_TMP_H, F
movlw 01H
addwf _X_TMP_L, F
btfsc _C
incf _X_TMP_H, F
return

Shifter ; Function for shifting 16bit signed REG i times
banksel _SHIFT ; left/right MUL/DIV
movfw _SHIFT ; INPUTS: FSR & IRP are pointing to LOW REG
andlw 0x0F ; SHIFT = d000 XXXX where XXXX = # of shifts
btfsc _Z ; and d = dir of shift
return ; check for zero interations
movwf _Counter1
btfss _SHIFT,0x7
goto CORDIC_MUL

CORDIC_DIV
rrf INDF, F ; Divide INDF/2 INDF --> low byte
bcf INDF, 7
decf FSR, F
rrf INDF, F
incf FSR, F
btfsc _C
bsf INDF, 7 ; Carry from the upper byte to the lower byte
decf FSR, F
bcf INDF, 7 ; Check to see if TEMP should be a positive number
btfsc INDF, 6
bsf INDF, 7 ; Was a negative number, force it negative again
incf FSR, F
decfsz _Counter1, F
goto CORDIC_DIV
return

CORDIC_MUL
bcf _C
rlf INDF, F
decf FSR, F
rlf INDF, F
bcf INDF, 7
btfsc _C
bsf INDF, 7
incf FSR, F
decfsz _Counter1, F
goto CORDIC_MUL
return
;_________________________________________________ _____________________________
endasm

[mister_e]

Carefull when using Banksel and PBP, i would suggest you to replace all BANKSEL for CHK?RP

CHK?RP is to PBP what BANKSEL is to ASM.

Using BANKSEL will make weird things and lost compiler because he don't know where he was before etc etc... i understand why... i just can't explain it correctly.

Darrel could you shed some lights and explain it in english :D

[ScaleRobotics]

Thanks a lot mister_e,

I added your suggestion, which will probably help in the long run. However, I still get the same result for Cos(x) = 71 no matter what I give as x. When edited out of some of the lines that had "global" in them, as suggested in an earlier post, was I supposed to replace "global" with something else? Here are some of the lines I deleted from various places in the program:

GLOBAL X_L, X_H, Y_L, Y_H, Z_L, Z_H, SHIFT
GLOBAL ATAN
GLOBAL SINCOS
GLOBAL Shifter

I don't really understand what these lines did in the original assembly code.
I also wonder if I defined any of the variables wrong. RV, Dir, _C, and _Z are defined inside the assembly code, and the other variables are named in the pbp code. Strange that the ATAN functions great, just the SINCOS having difficulties.

[Darrel Taylor]

The Input to SINCOS is the Z variable. (Not X)

I've got it working here, but it seems like it's not right.
Input range is from -256 to +255.

I've plotted the results. Does it look right to you?

<img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2133&stc=1&d=1195338956">
<br>

[ScaleRobotics]

Actually, it is perfect!!!!
They do something weird to signify the angle. For actual angle z, degrees = z * 90/256, or z/2.844444444444 = degrees. Output/1000 = the fractional result.

Attached is part of their excel spread sheet showing inputs and outputs vs actuals.
<img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2134&d=1195310172">
Many thanks for your time,

Walter

[Darrel Taylor]

For actual angle z, degrees = z * 90/256

That helps! Now the SIN and COS results all fall into place.

But, still having problems with the ATAN function.
Can't seem to get anything meaningfull out of it.
And that spreadsheet won't run for me, so I can't tell what it's supposed to do.

Do you have any examples for that too?
<br>

[ScaleRobotics]

I have another excel example that I will post (when I get to my other computer).This one can't look at it either. But basically, it works like this:

Enter X and Y co-ordinates like x = 12631 and y = 11881, then perform atan.

<img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2136&d=1195349246">

The accuracy shows that when either x or y is really low, that errors in the way it simplifies the equation are a bit large. But with higher numbers, the accuracy is within one degree or so.

Result pops out as Z. Z is the angle to the co-ordinates. (if I remember right the answer was 134?), but it is in the same weird format. Z * 90 / 256 gets you back to a readable degree format, which if I remember right give you about 47 degrees. You will have to know what quadrant you are in to figure out if you add 90 degrees to result by figuring out if x or y or both are positive or negative.

I made the 20 mhz PIC do 10,000 of these calculations, and it took less than 5 seconds. Over 2000 per second! Pretty cool!
I wish I got this excited about trig 22 years ago, when I was taking the class ......

[mister_e]

looking at the asm... i'm aware that my asm skills sucks, but, i don't understand why they used the line in red...


ATAN
banksel _Z_H ; Z_H & Z_L is loaded with 0x0000
movlw 0x00 ; Y & X is the input
movwf _Z_H ; X(i+1) = Xi - Yi*_Di*2^-i
movlw 0x00 ; Y(I+1) = Yi + Xi*_Di*2^-i
movwf _Z_L ; Z(i+1) = Zi - _Di*ATAN(2^-i)
banksel _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0
bsf RV ; Set Vectoring Mode
goto CORDIC_Alg

SINCOS
banksel _Y_L ; Z_H & Z_L is the input
movlw 0x00 ; Y_H & Y_L is loaded with 0x0000
movwf _Y_L ; X_H & X_L is loaded with 6070 (1/An constant)
movlw 0x00
movwf _Y_H
movlw LOW .6070
movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i
movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i
movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i)
banksel _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1
bcf RV ; Set Rotational Mode

w already hold 0x00 and they reload it again for the next MOVWF? BTW i would use CLRF instead... no big deal, it remove 4 instructions... long run, it may decrease a little bit the calculation latency. OK OK no big improvement, but... i couldn't resist. maybe i'll find something else?

so, once modified, the above code section should look like...


ATAN
CHK?RP _Z_H ; Z_H & Z_L is loaded with 0x0000
; Y & X is the input
clrf _Z_H ; X(i+1) = Xi - Yi*_Di*2^-i
; Y(I+1) = Yi + Xi*_Di*2^-i
clrf _Z_L ; Z(i+1) = Zi - _Di*ATAN(2^-i)
CHK?RP _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0
bsf RV ; Set Vectoring Mode
goto CORDIC_Alg

SINCOS
CHK?RP _Y_L ; Z_H & Z_L is the input
; Y_H & Y_L is loaded with 0x0000
clrf _Y_L ; X_H & X_L is loaded with 6070 (1/An constant)

clrf _Y_H
movlw LOW .6070
movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i
movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i
movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i)
CHK?RP _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1
bcf RV ; Set Rotational Mode


But yeah, i think we're not at the optimization stage yet ;)

[ScaleRobotics]

Looks great! Good catch.

Here is a paste able version of the code to integrate into picbasic. (just takes up about 180 words). It loads the bytes needed for assembly. It uses X_ Y_ and Z_ word variables to load the bytes needed for the assembly routine.



'************************************************* ***************
'* Name : trig-pic16.inc *
'* Author : Walter Dunckel *
'* Notice : Copyright (c) 2009 Scale Robotics Inc. *
'* : All Rights Reserved *
'* Date : 9/6/2011 *
'* Version : 2.1 *
'* Notes : Consolodated some variables to save ram and space *
'* : *
'* to convert degrees to radians Z_ * 256 / 90 *
'* to convert radians to degrees Z_ * 90 / 256 *
'* *
'* to perform ATAN, enter values for X_ and Y_ : *
'* X_ = 12631 *
'* Y_ = 11881 *
'* call ATAN *
'* 'result will be in Z_ radians = 134 *
'* Z_ * 90/256 = 47 degrees *
'* *
'* To find the result of Cos and Sin: *
'*Z_ = 90 'put angle in Z_ in degrees *
'*Z_ = (Z_ * 255)/90 'convert degrees to radians *
'*call SINCOS 'perform sin and cos on Z_ value *
'* 'results will be located here: cos(Z_) = X_ :sin(Z_) = Y_ *
'* in thousandths, for instance 9999 ~ 1.000 *
'************************************************* ***************
'more info: http://www.picbasic.co.uk/forum/showthread.php?t=7502
'and PIC18 version http://www.picbasic.co.uk/forum/showthread.php?t=10528

X_ var word bank0
Y_ var word bank0
Z_ var word bank0
X_H VAR X_.byte1
X_L VAR X_.byte0
Y_H VAR Y_.byte1
Y_L VAR Y_.byte0
Z_H VAR Z_.byte1
Z_L VAR Z_.byte0
X_TMP_H VAR byte bank0
X_TMP_L VAR byte bank0
Y_TMP_H VAR byte bank0
Y_TMP_L VAR byte bank0
ITERAT_TMP VAR byte bank0
ITERATION VAR byte bank0
Di VAR byte bank0
COSCH VAR byte bank0
COSCL VAR byte bank0
Counter1 VAR byte bank0
SHIFT VAR byte bank0


'***********************************
goto jump_trig


asm
;_________________________________________________ _________________________
FSR_ptr macro addr
CHK?RP addr
bcf STATUS,IRP
btfsc STATUS,RP1
bsf STATUS,IRP
movlw LOW addr
movwf FSR
endm

#define Dir _Di,0
#define RV _Di,1
#define _C STATUS,0
#define _Z STATUS,2

Z_stepH_tbl
addwf PCL,f
dt 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
Z_stepL_tbl
addwf PCL,f
dt 0x80,0x4C,0x28,0x14,0x0A,0x05,0x03,0x01
Z_NstepH_tbl
addwf PCL,f
dt 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
Z_NstepL_tbl
addwf PCL,f
dt 0x80,0xB4,0xD8,0xEC,0xF6,0xFB,0xFD,0xFF

_ATAN
CHK?RP _Z_H ; Z_H & Z_L is loaded with 0x0000
clrf _Z_H ; Y & X is the input
clrf _Z_L ; X(i+1) = Xi - Yi*_Di*2^-i
; Y(I+1) = Yi + Xi*_Di*2^-i
; Z(i+1) = Zi - _Di*ATAN(2^-i)
CHK?RP _Di ; _Di = 1 if Yi < 0 ELSE _Di = 0
bsf RV ; Set Vectoring Mode
goto CORDIC_Alg
_SINCOS
CHK?RP _Y_L ; Z_H & Z_L is the input
clrf _Y_L ; Y_H & Y_L is loaded with 0x0000
clrf _Y_H ; X_H & X_L is loaded with 6070 (1/An constant)
movlw LOW .6070
movwf _X_L ; X(i+1) = Xi - Yi*_Di*2^-i
movlw HIGH .6070 ; Y(I+1) = Yi + Xi*_Di*2^-i
movwf _X_H ; Z(i+1) = Zi - _Di*ATAN(2^-i)
CHK?RP _Di ; _Di = 0 if Zi < 0 ELSE _Di = 1
bcf RV ; Set Rotational Mode
CORDIC_Alg
CHK?RP _ITERATION
clrf _ITERATION
Rotation_loop
BUFF_X_Y ; Buffers both X & Y into temporary registers
CHK?RP _Y_L
movf _Y_L, W ; Move Y --> T_TEMP
movwf _Y_TMP_L
movf _Y_H, W
movwf _Y_TMP_H
movf _X_L, W ; Move Y --> T_TEMP
movwf _X_TMP_L
movf _X_H, W
movwf _X_TMP_H
CHK?RP _Di ; Vectoring or Rotational Mode?
bcf Dir
btfsc RV
goto Rot
Vec
btfss _Z_H, 7 ; Determine the state of _Di (1 OR -1)
bsf Dir ; _Di=1 when Z_H is pos
goto Cordic
Rot
btfsc _Y_H, 7 ; Determine the state of _Di (1 OR -1)
bsf Dir ; _Di=1 when Y_H is neg
Cordic
movfw _ITERATION
movwf _SHIFT ; Load number of shifts
bsf _SHIFT,7 ; Shift right (Division)
FSR_ptr _Y_TMP_L ; Load FSR
call Shifter ; Execute shift
btfsc Dir
call COM_YTEMP ; Get the 2's complement of teh TEMP register
SUM_X ; Calculate X(i+1)
movf _Y_TMP_H, W
addwf _X_H, F
movf _Y_TMP_L, W
addwf _X_L, F
btfsc _C
incf _X_H, F
movfw _ITERATION
movwf _SHIFT ; Load number of shifts
bsf _SHIFT,7 ; Shift right (Division)
FSR_ptr _X_TMP_L ; Load FSR
call Shifter ; Execute shift
btfss Dir
call COM_XTEMP ; Get the 2's complement of teh TEMP register
SUM_Y ; Calulate Y(i+1)
movf _X_TMP_H, W
addwf _Y_H, F
movf _X_TMP_L, W
addwf _Y_L, F
btfsc _C
incf _Y_H, F
btfss Dir
goto ADD_Z
movlw HIGH Z_NstepH_tbl
movwf PCLATH
movfw _ITERATION ; Fetch appropriate value from table
call Z_NstepH_tbl
addwf _Z_H, F
movlw HIGH Z_NstepL_tbl
movwf PCLATH
movfw _ITERATION
call Z_NstepL_tbl
addwf _Z_L, F
btfsc _C
incf _Z_H, F
goto Rot_Loop_out

ADD_Z
movlw HIGH Z_stepL_tbl
movwf PCLATH
movfw _ITERATION
call Z_stepH_tbl
addwf _Z_H, F
movlw HIGH Z_stepH_tbl
movwf PCLATH
movfw _ITERATION
call Z_stepL_tbl
addwf _Z_L, F
btfsc _C
incf _Z_H, F
Rot_Loop_out
incf _ITERATION,f
btfsc _ITERATION,3
return
goto Rotation_loop
COM_YTEMP
comf _Y_TMP_L, F ; Complememt Y_TMP
comf _Y_TMP_H, F
movlw 01H
addwf _Y_TMP_L, F
btfsc _C
incf _Y_TMP_H, F
return
COM_XTEMP
comf _X_TMP_L, F ; Complememt X_TMP
comf _X_TMP_H, F
movlw 01H
addwf _X_TMP_L, F
btfsc _C
incf _X_TMP_H, F
return
Shifter ; Function for shifting 16bit signed REG i times
CHK?RP _SHIFT ; left/right MUL/DIV
movfw _SHIFT ; INPUTS: FSR & IRP are pointing to LOW REG
andlw 0x0F ; SHIFT = d000 XXXX where XXXX = # of shifts
btfsc _Z ; and d = dir of shift
return ; check for zero interations
movwf _Counter1
btfss _SHIFT,0x7
goto CORDIC_MUL

CORDIC_DIV
rrf INDF, F ; Divide INDF/2 INDF --> low byte
bcf INDF, 7
decf FSR, F
rrf INDF, F
incf FSR, F
btfsc _C
bsf INDF, 7 ; Carry from the upper byte to the lower byte
decf FSR, F
bcf INDF, 7 ;Check to see if TEMP should be positive number
btfsc INDF, 6
bsf INDF, 7 ;Was a negative number, force it negative again
incf FSR, F
decfsz _Counter1, F
goto CORDIC_DIV
return

CORDIC_MUL
bcf _C
rlf INDF, F
decf FSR, F
rlf INDF, F
bcf INDF, 7
btfsc _C
bsf INDF, 7
incf FSR, F
decfsz _Counter1, F
goto CORDIC_MUL
return
;_________________________________________________ _____________________________
endasm
jump_trig:

[ScaleRobotics]

Then you can do some pretty cool stuff!

Like find the bearing to and distance to a lat-lon waypoint (solving for latitude adjustment of Longitude!)

Here is my ugly code. Needs to be cleaned up ... but it works for waypoints that are within 3.2000 degrees of eachother (200 miles or so). Angle still needs to be corrected depending which quadrant the waypoint lies.



DEFINE __16F877A 1 ; list directive to define processor
DEFINE OSC 20 ' Define Oscillator Speed
DEFINE LOADER_USED 1

' Shutdown comparators
ADCON1 = 7 ; Turn of all A/D
CMCON = 7
' Set the port directions
' 0=output 1=input
TRISA=%00000011 ' Set PORTA 0-1-3 used for AD
TRISB=%11011100 ' Set PortB
TRISC=%10000000
TRISD=%00000000 ' Set PortC USART RX as an input
porta.2 = 0
porta.3 = 0

X_ var word
Y_ var word
Z_ var word
X_H VAR byte
X_L VAR byte
Y_H VAR byte
Y_L VAR byte
Z_H VAR byte
Z_L VAR byte
X_TMP_H VAR byte
X_TMP_L VAR byte
Y_TMP_H VAR byte
Y_TMP_L VAR byte
ITERAT_TMP VAR byte
ITERATION VAR byte
Di VAR byte
COSCH VAR byte
COSCL VAR byte
Counter1 VAR byte
SHIFT VAR byte
x var word
y var word
z var word
i var word
lon_dif var word
lat_dif var word
angle var word
angle_deg var word
distance var word
j var word

;---------[change these to match your hardware]------------------------------
DEFINE LCD_DREG PORTD ; Set LCD Data port B C D
DEFINE LCD_DBIT 0 ; Set starting Data bit (0 or 4) 4 0
DEFINE LCD_RSREG PORTA ; Set LCD Register Select port A A
DEFINE LCD_RSBIT 3 ; Set LCD Register Select bit 3 2
DEFINE LCD_EREG PORTA ; Set LCD Enable port A A
DEFINE LCD_EBIT 1 ; Set LCD Enable bit 1 5
DEFINE LCD_BITS 4 ; Set LCD bus size (4 or 8 bits) 4 4
DEFINE LCD_LINES 2 ; Set number of lines on LCD 2 2
'----------------------------------------------------------------------------
clear
lcdout $FE,1

pause 500
lcdout $FE,1,"Test"

' to convert degrees to radians Z_ * 256 / 90
' to convert radians to degrees Z_ * 90 / 256
Start:
x = 0
y = 0
z = 37 'input current latitude 37.xxx for longitude correction (lon smaller at higher latitude)
lat_dif = 11881 'difference between current position and waypoint in latitude 1.1881 deg
lon_dif = 15816 'difference between current position and waypoint in longitude 1.5816 deg
i = z * 256 / 90 'changes z to the right format +256 to -256
'each 1/3 degree should equal about 1 radian need to figure out how to get this accuracy......
if (z*256)//90 >= 45 then i = i +1 'if answer is .5 or more, go to next highest integer (round number)
Z_ = i 'put result in Z_
Call sincos
i = X_ * lon_dif ' X_ = cos(lat_current)
lon_dif = div32 10000 ' get the value of lon_dif * the fraction for lon correction (reduces lon_dif)

X_ = lon_dif 'load lon_dif into X_ , prepare for ATAN function
Y_ = lat_dif 'load lat_dif into Y_ , prepare for ATAN function

call atan 'perform ATAN assembly function

angle = Z_
angle_deg = Z_ *90/256 'puts angle in degrees into the variable

'Below code computes distance between waypoints
'Z_ is already loaded with angle from above result

call sincos
i = lon_dif * 10000 'get lon_dif ready to divide by fraction
distance = div32 X_ 'divide by cos(Z_)gives latitude distance in d.dddd or really ddddd
distance = distance * 69 'get miles units
distance = div32 100 'hundreths of miles xx.xx miles

lcdout $FE,1,#distance
lcdout $FE,$C0,#angle_deg 'not corrected for quadrant, ie. + 180, or + 90, etc. only bare triangle angle
end '***********************************

[bluesmoke]

For anyone interested, the latest code was posted by Scalerobotics here:

http://sites.picbasic.net/index.php?option=com_uhp2&Itemid=6&task=viewpage&user_id=91&pageid=62

(the downloads don't seem to be working right now)

I believe the 16 bit code as posted on the above link did need the following minor tweaks:

Instead of:

lathome_min = lathome_min + 33*((((gpsdata[char_A+4] -48)*1000)+((gpsdata[char_A+5]-48)*100)+((gpsdata[char_A+7]-48)*10)+(gpsdata[char_A+8]-48))*5)//3

However, I believe it should read as follows so that the modulus is calculated first:

lathome_min = lathome_min + 33*(((((gpsdata[char_A+4] -48)*1000)+((gpsdata[char_A+5]-48)*100)+((gpsdata[char_A+7]-48)*10)+(gpsdata[char_A+8]-48))*5)//3)


Second:

latdest_min = latdest_min + (tempbyte*33) 'use remainder and add to latdest_min

should read as follows:

latdest_min = latdest_min*5/3 + (tempbyte*33) 'use remainder and add to latdest_min