A Simplified Waypoint Navigation Equation walter@scalerobotics.com Ina search of the internet for navigation equations, I found none that could easily be handled by PicBasic. 1 saw some posts talking about how the PIC chips were not powerful enough to handle the equations, polar co-ordinates, floating point, blah, blah, etc. These “nay-sayers” are probably right. The real equations would probably overwhelm the PIC and at least for me, be very hard to write in PicBasic! ‘What needed to be done was to simplify the calculations, to make it easier for the PIC chips (and I) to process the equations. A simplified equation would not be as accurate as the real deal. But for most applications, being off by a degree or two might not pose a problem. I thought I would try to use some basic trigonometry to solve the equation of getting from one set of Longitude and Latitude, to another, or from waypoint to waypoint. This equation needed to solve the bearing to the next waypoint, as well as the distance to the destination waypoint. The following drawing shows the quadrant location, Center (of crosshairs) is current location, and the asterisk (“**”) is destination in this example. 0 Deg Quadrant 4 | Quadrant] -Latitude | - Latitude - Longitude | + Longitude 270 Deg 90 Deg Quadrant 3 | Quadrant 2 +Lat | +Latitude -Lon | +Longitude 180 Deg Our current position in Gilroy is 37.0026 deg N Latitude and 121.5827 W Longitude Our destination waypoint is 38.1907 deg N Latitude and 120.0011 W Longitude To find the difference between the two points, we take our current position, and subtract, the destination location, Like this: Latitude difference = 37.0026 — 38.1907 = -1.1881 And longitude difference = 121.5827 ~ 120.0011 = + 1.5816 Scale Robotics Inc. 1476 Senegal Ct Gilroy, CA 95020 www.uavs.net‘We will use the result of the difference in position to make a triangle representing the equation, We will get rid of the decimal point. 11881 (latitude) 15816 (longitude) But wait! ... The length of longitude changes depending on our latitude. This will screw Lup our equation, So we will have to compensate for that, At the equator Longitude and Latitude are the same, 69 miles per degree. As you get further North, Longitude gets shorter per degree, but Latitude is a constant length per degree. To compensate for this difference, we need to use a division-constant table. You ean see that as your latitude gets up to 70 degrees or so, things start to get pretty screwy. Fortunately for my equation, not many people live above 70 degrees, so for most people this solution will be accurate enough. Latitude IfLat>Lon If Lat Lon. * Rise Table: Rise Degrees (angle) Distance (hypotenuse) 0 0 12 1 5 12 2 9 12 E 4 12 4 18, B 5 23 13 6 7 B 7 30 14 8 34 14 9 37 15 10 40 16 a B 16 12 45 7 11, then our angle = 43 degrees according to the table. To get the bearing to the destination, we need to run through a couple checks. If Lat>Lon then If quadrant =1 then angle = bearing to destination If quadrant = 2 then 180 — bearing If quadrant = 3 then 180+ angle = bearing If quadrant ~ 4 then 360 —angle = bearing Endif Scale Robotics Inc. 1476 Senegal Ct Gilroy, CA 95020 ‘www ttavs.net