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