Efficient Fixed-Point Trigonometry For PIC16F

1 Attachment(s)

Microchip has an application note AN1061 http://ww1.microchip.com/downloads/e...tes/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

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 ...

Code:

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.

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.

Code:

;**************************************************************************** ;* 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

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

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.

1 Attachment(s)

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=119533895 6">
<br>

1 Attachment(s)

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

Quote:

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>

1 Attachment(s)

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 ......

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

Code:

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...

Code:

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 ;)

1 Attachment(s)

Cordic Trig Include file for PIC16

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.

Code:

'**************************************************************** '* 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:

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.

Code:

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 '***********************************

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

http://sites.picbasic.net/index.php?...d=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