Nav using GPS and the Prop: What to Expect?

Hey guys,

I'm slowly making progress on my project: the PropBot. This is a robot I built just to study the Propeller. I feel strongly about having a project as a reason to learn anything.·The objective of this robot has been to make an autonomous robot that navigates around the Parallax building parking lot via·four waypoints, avoiding any parked cars or square curbs·along the way. Ideally, I'll sit in my office and watch the bot lap the office every·two minutes or so till the battery dies.

I've made this work one step at a time. I've made each piece work on its own, then integrated them together into the Propeller. These pieces include two HB-25s, five Ping sensors and associated signal conditioning, and GPS. I'm not navigating via GPS, but I've got it working well in smart mode and displaying the data on TV or via Parallax Serial Terminal.

I'm ready to start navigating via GPS. And I seek any input you may have to help me get the best results. My approach is as follows:

Add HM55B compass for bearing
Put four GPS waypoints into Prop as constants
Do the math (using Float32) to calculate distance from existing location to waypoint (found some trig calcs to do this)
Calculate heading
Turn robot to the right direction
Advance towards waypoint, making the same calculation every few seconds
When waypoint is reached, increment to the next waypoint

I will also send coordinates back via radio using Phil's modem app.

The waypoints I choose should have about 10-15' on either side of the robot until square curbs or parked cars are encountered.

Cam's math library is essential to the project.

Any advice, from any perspective? I have such limited time for programming and I want to make the best use of my time.

Thanks,

Ken Gracey





·
<br>
Tagged:
«1

Comments

  • 38 Comments sorted by Votes Date Added
  • edited March 2009 Posts: 0Vote Up0Vote Down
    I am interested in the progress of your PropBot project. If you have a PPS GPS, its OK. But if you use a SPS one, as I suppose, to navigate precisely among cars when GPS error can be larger than a truck, is a major undertaking.

    Try to simulate a differential GPS by software. To do this you have to know the precise Lat, Lon, (Alt) coordinates of the starting position. Store the difference between the measured position and the starting position and use this difference during the travel to the other, precisely given or incremental, waypoints. This method assumes that the travel takes not more time than 2-5 minutes. To locate the first waypoint again you can use an IR seeker, for example, to home in and to update the GPS error for the next travel.

    By the way, how the cars will avoid the robot? (Well, during working hours there shouldn't be too much traffic there.) To begin an 'escape or avoid' sequence in time, you will need to make the position calculations and environment sensing more frequently than 'every few seconds'. Some emergency sound signal might be useful.·

    Can the hardware estimate the travelled distance or the angle of a turn from the rotation of the motors/wheels? That would help much.

    Some grid memory of the occupied territories can be useful. After that you can use Lee-algorithm to find shortest route among obstacles. On the next run the robot can re-position itself on the grid with some AI method.

    What if one puts a small but strong magnet on the robot by accident? In other words, the robot should be able to calibrate its magnetic sensor, sometime, or, the algorithm should recognize magnetic perturbations. (Wheels are running parallely, but sensed magnetization changes abruptly. Or, during a longer straight run, the difference between GPS and magnetic heading·is more tha acceptable.)
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Sparkfun sells a GPS IC that has all that your looking for built in. It gets Longitude, latitude, speed, time etc. No calculations are needed. here is the link
    www.sparkfun.com/commerce/product_info.php?products_id=8416

    I access it via FullDuplexSerial

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Visit my site -> [url=Http://www.rawcircuits.com]www.rawcircuits.com[/url]
  • edited March 2009 Posts: 0Vote Up0Vote Down
    One note of caution. Float32 does not have enough precision for normal calculations using the Spherical Law of Cosines method. I had to change the calculation to use the Haversine method. See the attached GPS Utilities archive.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Ken, you say "until the battery dies"... Why not have the robot navigate to a simple "charge station", and recharge it's batteries when they get low? This would really be autonomy! [noparse]:)[/noparse]
  • edited March 2009 Posts: 5,649Vote Up0Vote Down
    Hey all,

    Thanks much so far for the input. You've given me a lot to think about this afternoon, so I'll do a bit of reading before I do any programming.

    Philldapill, sure! Charging station would be cool. It's another project of complexity considering all that I've got going on with this robot already, but I'll save the thought and if I achieve the other portions of the project then a charging station will be added.

    Chuck and cessnapilot: also thanks!

    I'll be back with more questions after I have something to show.

    Ken Gracey
    <br>
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Wow Ken that's funny I'm actually working on *the exact same setup* as you. Parallax GPS, HM55B compass sensor, Propeller, using it to navigate a robot around a parking lot using four waypoints. You wouldn't happen to be going to the same Sparkfun autonomous vehicle competition www.sparkfun.com/commerce/product_info.php?products_id=9016 that I'm going to, would you? If you're not already coming, you should totally make plans and come. It's in Boulder, CO on April 15th.

    My robot actually uses 3 Propellers: one dedicated to proximity sensors, one main unit that also handles telemetry and motor control, and the third is strictly for navigation & GPS. I've actually added a Memsic 2 axis accelerometer to the setup you described, to keep track of motion for better position extrapolation between GPS updates. I realized I wasn't going to have enough cogs left over in the main unit's Propeller, so I put the navcomp (navigation computer) into it's own box with a dedicated Propeller and an LCD display. I actually just finished building the whole navcomp last Saturday. The nice thing about it is that I can take the navcomp box off the robot, stick a 9V battery in it, and test it separately from the robot by just walking around outside holding it!

    I got the LCD, accelerometer, and compass working yesterday, but I can't get any serial data from the GPS in raw mode, and I can't seem to find an object on the object exchange for running the GPS in smart mode--? I added Beau Schwabe's transistor level shifter after reading a forum thread where he says the GPS module requires 4 volts for an "on" bit so the Propeller can't Tx to it through a resistor alone. Maybe I made a mistake building the level shifter circuit, or perhaps I just need to cut the level shifter out and use the GPS in raw mode with a resistor alone. What kind of circuit are you using with your GPS?

    Ken, why don't we collaborate on the code? I've got my compass working, you've got your GPS working, we can put those together and work on waypoint navigation. We could make a general purpose navigation computer object that can be configured for custom setups or used on the QuadRover.

    Instead of navigating blindly to waypoints, I'm thinking of the waypoints as describing lines that the robot tries to follow. That way if it can't physically make it to the waypoint (due to obstacles and/or GPS inaccuracies) it will settle for following parallel to the line until a path clears, trying to get closer to the line only if it can, so it will continue to advance rather than stopping at one bad waypoint. You could think of the lines as describing a potentially infinite number of waypoints, so that the robot follows a course rather than trying to get to a fixed point.

    A friend of mine suggested that if an obstacle is in the path, you could add temporary waypoints to make a box around the obstacle, which would cause the robot to follow a rectangular path around the obstacle before returning to the path. So some functions of the navigation object might be:

    Init()
    SetWaypoint()
    GetDirectionToWaypoint() ' degrees; waypoints are points
    GetDistanceToWaypoint()
    GetDirectionToPath() ' waypoints are lines
    GetDistanceToPath()
    AvoidObstacle(direction) ' Generate temporary path. direction = degree heading where obstacle was detected
    Backtrack() ' Try to follow the temporary course backwards to get back to the main path

    I anticipate we might want to implement different navigation strategies on top of the basics; if we disagree on the algorithms that's good because it means we'll generate a more general purpose code that can be used by others too. What we can do is, we make a basic waypoint navigation object that does the lowest common denominator, and we can wrap that object with extra algorithms if we want.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    I am using approximately the same setup, but I have 4 proto-boards. $Navboard, $MotorBoard, $Comboard, and $SenBoard. I am using the Parallax compass, Parallax GPS, Parallax wheel and encoder kit, (with non-Parallax H-Bridge). I use the GPS in raw format, and I do not use the interface resistor you mention. The Communications board is connected to the other three serial boards, and can connect to my laptop via X-Bee. I also have a servo mounted sonar sensor that scans for obstacles. The pieces are all working fine. I am now working on tying all the pieces together which is the hard part. I doubt that I will be ready for the SparkFun contest (and it is a long way from Texas), but I have been working a long time on this robot and hope that someday I will get it rolling. I am also planning to add a camera which may take another propeller and a Wi-Fi connection just for web control/access.

    So many ideas, So many projects, So little time.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Thanks Chuck.

    I think the GPS module has an integrated 220 ohm resistor on the data line, which is just enough to keep it from damaging the Propeller or itself, but might be a little on the small side (usually you would use 1-2K I think). But then, what do I know: my setup currently isn't working! I might just try connecting it directly. It is, after all, only a worst case draw of 10 mA, and I'm sure the SX inside the Parallax GPS unit can handle it.

    Are you using the GPS float objects from the OBEX? They have separate constants for Tx pin and Rx pin - do you just set them to be the same pin? I was wondering if maybe that code won't work if the Tx and Rx pins are the same.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    I use a modified GPS_IO_Full See attached). This code is a work in progress, so it is not clean yet. Don't look too closely! It is also based on lots of other work. I still need to add attributions.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Dr. Anderson, a member of the Dallas Personal Robotics Group, has some of the most impressive robots I have ever seen. Here is one link to some tutorial infromation.

    http://geology.heroy.smu.edu/~dpa-www/robo/challenge/examples.html

    I have a file of his tutorials, but they are 10,000 miles away.

    John Abshier
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Ken,

    I'm trying to do the same thing.· In order to avoid the intimidating Latitude and Longitude numbers, I use Google Earth to select a point just to the Southwest of the area where my robot will be running.·I call this point the "local origin".· All of my GPS readings are measurements relative to that point.· Also, the meters per degree Latitude and Meters per degree Longitude are calculated at the local origin.· All of these numbers are manually entered as constants in my spin program.

    Last weekend I was at CIRC Bot Brawl in Peoria and gave a static display of my robot.· The attached PeoriaCalc is the Excel calculations for the local origin and several other points in the area where the Bot Brawl was held.· Peoria Spin is the list of constants that were entered.

    One note here.· I have chosen to use decimeters as a standard in my program for all of the measurements.· I know that it is rather optimistic to use decimeters for GPS measurements, but it works well for sonar and encoders, and I didn't want to worry about length conversions in he program.

    The last file, ReadNema.spin, is the program that reads the GGA and RMC sentences from an EM-406A at 56kbits.· It saves the fix and # of satellites from the GGA and Latitude, Longitude, Speed over ground, and Course over ground from the RMA sentence.· Note that the atoi calculations are only for the differences between the present location and the local origin and that the distance is calculated at the same time.

    The last piece of the puzzle is the arcTan.· I'm using some code that Mike Green posted awhile back.· It is accurate to within one degree most of the time, which is good enough for me.

    The code was working very well last fall.· I'm just waiting for warm, dry days to start playing outside again.

    ······ Rick Brooks

    Post Edited (Rick Brooks) : 3/10/2009 6:49:56 AM GMT
  • edited March 2009 Posts: 0Vote Up0Vote Down
    This might be a good spot to post this may help you/others too..

    Im doing a bit of gps nav myself here... Im starting to understand it sort off...

    I'm working on the first step which is converting the·lat / lon·from ddmm.mmmm to dd.dddd
    i found some great pages with help. my code i think is basically·moving·in the·right direction·but the math is pretty off when i check it with an online converter.. basicaly this is the basic math im doing. Note: this isnt the exact syntax i just stripped out the float stuff to make it easier to understand.

        'latitude := 3819.5882 
        'lat_dd := trunc(latitude / 100)
        'lat_mm := lat - (lat_dd * 100)
        'lat_mm := lat_mm / 60
        'lat_dec := lat_dd + lat_mm
    
    

    my lat is say.. 3819.5882 from my gps
    my decimal output from the·previous basic code is.. 38.3265 ish (i say ish cuse the lat from the gps bounces a little obviously). This is much·different from online converters which give me around 38.332875.

    I followed online procudures to get the translation.. But im guessing it might have something to do with not taking into account another decimal place.. Am i supposed to be figuring out the extra decimal place ex:· 3819.58*.*82· ?

    did that make sense? [noparse]:)[/noparse]
    hope so thanks
    Jesse


    ·
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Ken Gracey said...
    I'm ready to start navigating via GPS. And I seek any input you may have to help me get the best results. My approach is as follows:

    Add HM55B compass for bearing ...

    Ken, I don't think you need the compass. The GPS system knows where North is.

    Jack
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Jack, don't all GPS's actually estimate your heading by looking at where you were at one point in time vs. where you are at the next point in time? That estimate wouldn't work when your robot is executing a turn in place.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    This is the bearing code that I came up with for my magic compass project. I also used two points on google earth to verify the results. Rotating the destination point (lat2/long2) around the starting point (lat1/long1)
    It uses the F32_full object


    lat1 := 28.963105 'coordinates have north bearing 0
    long1 := -81.471019
    lat2 := 28.963763
    long2 := -81.471019


    r := F32.atan2(f32.fsub(F32.fmul(F32.cos(f32.radians(f32.fsub(90.0,lat1))),F32.sin(f32.radians(f32.fsub(90.0,lat2)))),F32.fmul(F32.fmul(F32.sin(f32.radians(f32.fsub(90.0,lat1))),F32.cos(f32.radians(f32.fsub(90.0,lat2)))),f32.cos(F32.Radians(F32.Fsub(long2,long1))))),F32.fmul(F32.sin(F32.Radians(F32.Fsub(long2,long1))),F32.cos(f32.radians(f32.fsub(90.0,lat2)))))


    degree := F32.degrees(r) ' degree comes out 0:east -90:north 90:south 180,-180:west

    Degree comes out as a negative number between 0-180 for the northern half
    and a postive number 0-180 for the southern half, both start at east and go to +-180 West.

    I used an if statement to covert it into 360 degrees rotation with north 0/360

    It seems fairly accurate.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Thomas, awesome thats gonna be a huge help almost too much help[noparse]:)[/noparse]
    Would you mind sharing your code to convert from ddmm.mmmm to dd.dddd? seems simple enough but as you can see, a couple posts up i seemed to have even messed that up.
  • Chris SavageChris Savage Parallax Engineering
    edited March 2009 Posts: 0Vote Up0Vote Down
    Dennis Ferron said...
    Jack, don't all GPS's actually estimate your heading by looking at where you were at one point in time vs. where you are at the next point in time? That estimate wouldn't work when your robot is executing a turn in place.
    Dennis,

    ·· The heading provided in the $GPRMC string is calculated in this manner.· But like altitude, it can vary in accuracy and so high end GPS system include dedicated compass and pressure sensor for altitude.· Take care.

    ▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
    Chris Savage
    Parallax Engineering
    Chris Savage
    Engineering Tech, Parallax Inc.
    (916) 624-8333 x3005
    Website / Twitter / Google+
  • edited March 2009 Posts: 0Vote Up0Vote Down
    s2jesse said...
    Thomas, awesome thats gonna be a huge help almost too much help[noparse]:)[/noparse]
    Would you mind sharing your code to convert from ddmm.mmmm to dd.dddd? seems simple enough but as you can see, a couple posts up i seemed to have even messed that up.

    Haven't got there yet. I don't even have a GPS unit yet.

    I was hoping I could get the coordinates out of the GPS in dd.dddd format. But now I am guessing NOT?
  • edited March 2009 Posts: 0Vote Up0Vote Down
    most gps will output it in ddmm.mmmm string and you convert it with some basic math a few posts up.. its totally working ill give you my code once i figure out that little question i had up there with the extra decimal place..
  • edited March 2009 Posts: 0Vote Up0Vote Down
    'latitude := 3819.5882
    'lat_dd := trunc(latitude / 100)
    'lat_mm := lat - (lat_dd * 100)
    'lat_mm := lat_mm / 60
    'lat_dec := lat_dd + lat_mm

    What is the lat variable on the third line?
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Dennis Ferron said...
    Jack, don't all GPS's actually estimate your heading by looking at where you were at one point in time vs. where you are at the next point in time? That estimate wouldn't work
    when your robot is executing a turn in place.

    Yep, you're absolutely right, Dennis. About a second I posted my message, I realized that the GPS, though it does indeed know where North is, it doesn't know
    your attitude <mine is awful some days [noparse];)[/noparse]>. If you're moving, it knows your velocity, so you can infer your heading. But if you're standing still, it can't tell your heading.

    You just pointed out a case I hadn't even thought of, which is turning in place. Airplanes and missiles don't do that <grin>.

    Ken, forget I even posted. Everything I said was wrong; you do need a compass.

    Jack
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Thomas Fletcher said...
    'latitude := 3819.5882
    'lat_dd := trunc(latitude / 100)
    'lat_mm := lat - (lat_dd * 100)
    'lat_mm := lat_mm / 60
    'lat_dec := lat_dd + lat_mm

    What is the lat variable on the third line?
    oops sorry it should be


    'latitude := 3819.5882 
    'lat_dd := trunc(latitude / 100)
    'lat_mm := [b]latitude[/b] - (lat_dd * 100)
    'lat_mm := lat_mm / 60
    'lat_dec := lat_dd + lat_mm
    
    

    and to hammer it in one more time for clarity... thats·basically the·right math to convert it, but i think i have to do something extra because when you enter a lat into an online converter it makes you split up the 3819.5882· into somthing like· 38 19 58.82· and im not taking into account that last decimal. but im probably just totally clueless what im supposed to do with it.

    heres a new page i just found with some good info though... http://home.online.no/~sigurdhu/Deg_formats.htm


    Post Edited (s2jesse) : 3/11/2009 5:38:45 AM GMT
  • edited March 2009 Posts: 0Vote Up0Vote Down
    update... fyi i think that math is totally correct ill verify later... my problem was i was checking my work using an online converted that expected a different format... ugg [noparse]:)[/noparse]
    so anyway that should be correct for anyone who needs to convert their ddmm.mmmm to dd.dddd
  • edited March 2009 Posts: 0Vote Up0Vote Down
    I've haven't done any GPS math or navigation on the Prop yet; therefore allow me to speak freely on the subject. smile.gif A couple things come to mind:

    1. Why bother with floating point at all? Couldn't you just take 123.8854 degrees and ignore the decimal point altogether, leaving 1238854? Better yet, as another poster suggested, pick a reference point, say 123 degrees, and subtract it from all your readings, leaving 8854 in the example above. Unless you're navigating the world, you dont need those extra digits.

    2. Arctangent: who needs it? The HM55B provides direction as (x,y) (i.e. cos/sin) coordinates. The difference between where you are and where you want to be is also specified in (x,y) coordinates. If you're pointed toward your target, the direction sines and cosines between your heading and the target bearing will be equal. If they're not, you can figure out which way to correct without knowing the actual angle. These trig identities will give you the turn information:

    ····sin(turn) = sin(bearing - heading) = sin(bearing)cos(heading) - cos(bearing)sin(heading)
    ····cos(turn) = cos(bearing - heading) = cos(bearing)cos(heading) + sin(bearing)sin(heading)

    ... no actual angles or arctans required! Besides, once you start dealing with actual angles, modulo arithmetic (e.g. 270 + 180 == 90) rears its ugly head.

    -Phil
    “Impossible is just a big word thrown around by small men who find it easier to live in the world they’ve been given than to explore the power they have to change it. Impossible is not a fact. It’s an opinion. Impossible is not a declaration. It’s a dare. Impossible is potential. Impossible is temporary. Impossible is nothing.” –Muhammad Ali
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Phil,

    You are correct on all points.

    1. Simplify the GPS math with a reference point.· Remember that floating point normally gobbles up a spare cog or two.· Once the project gets close to being finished, you will need all of the cogs that you can get.

    2.· ArcTan is not required.· I would not use it if it took up extra cogs or took too much time.· The arctan function that I use is simple, reasonably fast, reasonably accurate, and does not eat cogs.· The Lego RoboMagellan competitors use a vey simple method that ignores all trig functions (uses just x and y differences) and is very, very·inaccurate, but it works surprisingly well.

    Modulo arithmetic must be considered in your programming.· Also, make sure that your functions work in all four quadrants.· Finally, watch for strange things happening at 0, 90, 180, or 270 degrees.

    ··· Rick Brooks
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Thomas Fletcher (or a forum moderator): can you please edit your post to put some line breaks in that long math expression in your Spin code? You've made the horizontal width of this thread explode.

    Fixed my GPS connection. After tearing down my level shifter circuit, trying direct connection to different Propeller pins, and even analyzing it all on a scope, finally realized that in fact the problem was that I had the Tx and Rx pins set to be the same in the GPS Float object. The driver sets the tx pin high, then the GPS can't pull it low.

    I quite agree with Phil, it seems remarkable to me that someone would haul in floating point on a microcontroller program just to handle what is basically a cartesian rectangular coordinate system. For gosh sakes people it's the same coordinate system used to call out squares in the board game "Battleship"; there's no reason to make it complicated. Just because there is a "." in the lat/longitude strings, does not mean it has to be floating point! Furthermore, why represent values in degrees, requiring a divide by 60? Why not use minutes as the basic unit, then you can multiply degrees by 60 and add to the minutes.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    So here's a way to do a simple GPS nav object: convert the DD MM.MMMM to units of ten-thousandth's of a minute so it's an integer. Choose a local origin to subtract from this number to get an X,Y offset from the local origin - for the kind of small distances a robot travels, we can ignore the curvature of the earth. Then we simply multiply this value by a constant to convert it to centimeters. For instance, where I live, one ten-thousandth of a minute of longitude is about 15 cm. So if one GPS update told me I was at 97°28.9000'W and the next told me I was at 97°28.9002'W, then I can see that xxxx.9002 - xxxx.9000 = 2, multiplied by 15 tells me I traveled 30 cm.

    Edit: You can find out the ratio of minutes to cm using this online calculator: www.ngs.noaa.gov/cgi-bin/Inv_Fwd/inverse2.prl. It will be different for different latitudes, so you would adjust a CON section in the object for what part of the world you're in. The way to get the calculator to tell you the ratio is to pick a GPS lat/long near where you are, and enter it, then for the second point just enter that same number +.0001 on the latitude. The distance it calculates for you is your 1/10000th of a minute to cm ratio; repeat same process for longitude.

    Post Edited (Dennis Ferron) : 3/14/2009 1:47:24 AM GMT
  • edited March 2009 Posts: 0Vote Up0Vote Down
    Chuck,
    Great work on your utilities.. I· spent like 2 days trying to figure out the spherical Law of Cosines method as well. It was driving me nuts. The haversine meathod works like a charm! The distances are accurate to within what apears the gps's error margins.
    After that I was trying out your bearing code as well but i cant seem to get·clean output from it. Im converting the miles formated distance back to rads using... (not concerned with optimzing anything at this point obvioulsy [noparse]:)[/noparse]
    PUB milesToRads(input)| t
      t:=fm.fdiv(input,1.150779) 'miles >> nm
      return fm.fdiv(t,3437.7387) 'nm >> rads 
    

    ·before i feed it into the bearing code but i seem to output 6.805647e+38.·Have you succuselfully got the bearing code to work for you in your spin code?
    Chuck Rice said...
    I use a modified GPS_IO_Full See attached). This code is a work in progress, so it is not clean yet. Don't look too closely! It is also based on lots of other work. I still need to add attributions.
  • edited March 2009 Posts: 0Vote Up0Vote Down
    I think that I had the calculation wrong. I should have been using the
    longitudinal distance only. I rewrote the bearing routine, but I do not
    have a board right now to test on. Give the following a try if you get
    a chance. My math skills are pretty rusty, so if there is a mathematician
    in the crowd, an algorithm check would be welcome.

    pub calculateBearing(lat1,lon1,lat2,lon2) : bearing | t1,t2,t3,x,y,dLon
    '' lat1,lon1,lat2,lon2 are floating point variables in radians
    '' Returns bearing in floating point radians
    
      dLon := fp.fsub(lon2,lon1)
    
      t1   := fp.fmul( fp.cos(lat1) , fp.sin(lat2) )
      t2   := fp.fmul( fp.sin(lat1) , fp.cos(lat2) )
      t3   := fp.fmul(     t2       , fp.cos(dLon) )
    
      x    := fp.fsub( t1,t3 )
      y    := fp.fmul( fp.sin(dLon) , fp.cos(lat2) )
    
      bearing := fp.atan2(y, x)
    
      return bearing
    
  • edited March 2009 Posts: 0Vote Up0Vote Down
    you the man!
    Seems right on the money! verfied my output with google earth.
Sign In or Register to comment.