Efficient Fixed-Point Trigonometry For PIC16F

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