Steering between waypoints

GPS/GNSS Development Boards
1

Hello

Can anyone please help me with GPS navigation between Latitude & Longitude waypoints. I have a model boat & need to control the steering servo.

Thanks.

2

You will need the RMC NMEA message.

  1. convert lat/lon of both the current position and the target waypoint to a local north/east frame, in meters.

  2. subtract the NE coordinates of the target from both. This makes the target 0,0 and the current position becomes relative to the target (N,E) in meters.

  3. Compute the modulus by Pythagoras. If it is less than some value, do nothing (because you are passing over the target).

  4. Otherwise, get the current direction (Track Angle from the RMC message).

  5. Compute the angle to the target (=arctan(current.E/current.N)), convert to degrees

  6. Subtract one angle from the other, giving you the correction needed

  7. Translate correction into proportional (or PID) left or right rudder

I’d do a few paper simulations to check for correct signs (+/-) first 8)

E

3

Check out the ardupilot project.

It has some very well commented code that you can learn from, or use.

http://diydrones.com/profiles/blogs/ardupilot-main-page

4

The first decision you need to make is the style of navigation you want to use. The simplest is just figure out which way to point the nose to get to where you want to go. The second is “am I on the line between where I started and the target, if not how do I get back to this line?”. In both cases you get to where you want to go, but in the first case, wind/wave/tide could push you way of the rhumb line. In the second, you will stay on the rhumb line, but the boat’s nose way not point anywhere close to the target location. For pilots this is the difference of flying using an NDB versus a VOR. The math for each is quite different.

5

fll-freak:
just figure out which way to point the nose to get to where you want to go.

fll-freak,

Which way the nose points is not part of the setup, unfortunately, nor is its measurement: I assumed there is no compass, either magnetic or gyro. Only GPS;

6

Thanks for your comments.

I intend to have a compas, and have a system like this:

Sit the boat near water, and point it to where the first waypoint will be.

Arduino will read compas, GPS and a distance value from a potentiometer (100m to 2000m).

Calculate the first waypoint from these values.

Calculate second waypoint (fixed distance to left or right of first WP).

The third waypoint is the starting point.

Has anyone done anything like this?

Could anyone write me a sketch for a small consideration.

Thanks.

7

elevator4:

fll-freak:
just figure out which way to point the nose to get to where you want to go.

fll-freak,

Which way the nose points is not part of the setup, unfortunately, nor is its measurement: I assumed there is no compass, either magnetic or gyro. Only GPS;

Very true. The “figure out part” would entail the taking the delta lat, delta lon, converting to feet (remembering to account for pie shaped longitude), doing the trig. Now you know the heading to your target. Without a compass, you need to computer your heading from the changes in alt/lon from moment to moment.

With a compass, once you know the destination heading you can use that to “point your nose”, unless you want to drive a course (VOR) then the math gets more complicated.

I once wrote an airtraffic control simulator for a large company. All the NDB/VOR math for simulating how pilots fligh flight plans was a royal pain. This is the same problem.

8

It does have a compas.

The compas reading is stored when the boat is sitting on shore pointing towards WP1 (first action when powered up).

The GPS will be read (values stored as Home) immediately after the compas, and the distance to travel to WP1 has been by a potentiometer representing meters up to (say) 2000. WP1 will always be 90 degrees to the shore.

There is one other WP to get to before returning to Home, but I will leave that for now.

I thought that given these three values, one could calculate the Lat & Long of WP1 and steer towards it.

But this is where I need help. ie a sketch to do the calcs for WP1 & sketch to control steering servo to get there.

Thanks for your interest in my problem.

9

I think I am missing something…

Your device has a magentic compass and a GPS. Given the coordinates (in lat/lon) you can write an algorithm to steer the boat to that location.

Do you need help with the algorithm, or with determining the location of the waypoint?

If you have a waypoint, why do you need to enter the distance?

My guess is that you do not know the lat/lon of the waypoint you want the boat to get to. It would be easier to enter a table of waypoint you want the boat to get to. Once it gets to within 50 feet of one, it would switch to the next till you exuast the table.

If you do not know the lat/lon of the waypoint, you can use the Vincety algorithm to compute it. There are several websites that will do that for you. Enter your start lat/lon, the initial azimuth, and the great circle distance, and it will tell you where you end up.

Now that you have a destination lat/lon, and you know your current lat/lon you can do the subtractions to get the delta lat/lon, convert into distance units (6080 feet is one minute of latitude; longitude is for shortened by the cosine of latitude), do the trig (arc tan) and get the heading. Since you have a compass, you can use a PID type loop to run the boat along that compass route. This will of course be updated every few seconds as the boat gets off course and then pulled back on by the PID.

Is this what you had in mind? If not, please give more details. We can’t ready minds (yet).

10

Sorry Guys

I have not made it clear yet.

The boat sits on the shore pointing to the first waypoint. At this time the waypoint has not been calculated because the Arduino has not been powered up. The waypoint is not pre-calculated because when you get the boat the the launch location there may be some reason why it can not be launched at the pre calculated position (because someone is fishing there).

So all of the (two) waypoints have to be calculated from the start GPS location (Home), the compas angle, and the preset direct distance to WP1.

The preset distance is taken from an potentiometer with 0 to 5V representing 100m to 2000m).

This has to be done on the spot by the Arduino no matter where the start point is.

Hope this helps. I have a full flow chart of required functions I could email directly to anyone who is able to assist.

Thanks for your interest.

11

So you have two problems to overcome.

  1. Calculate the waypoint based on current location, azimuth, and range. You can use the Vincenty algorithm (assumes an ellipsoidal earth) or a simple flat earth calculation. Since you are not likely going more than a mile, I would do this as a flat earth model. You still have to account for longitude forshortening.

  2. An algorithm to steer the boat to a waypoint.

What do you need help with?

12

From a private email, it would seem you need guidance with both aspects. From what I understand, you want your boat to leave the shore, travel a programable distance perpendicular to the shore, turn 90 degrees, travel 50 feet or so, and then return home.

You boat will have a pot, button, GPS, and a compass both connected to an Arduino. The Arduino will control the rudder and possibly a throttle.

As in all things there are a dozen ways to skin this cat. The simplest will be to start “flying an NDB”. In other words just pointing the nose in the direction you want to go to. You will not follow a straight line if you have a current or wind, but the math will be simpler.

Steps:

  1. After power on wait till the GPS locks

  2. Cache the lat lon

  3. Wait for a button press to cache the heading. That will be the direction to travel.

  4. At the same time read your pot for the distance to travel.

  5. Compute the target location (WP1). This is some yucky math but nothing horrible.

  6. Compute the second location (WP2).

  7. Wait for a button press to start the trip.

  8. Engage the throttle and navigate to WP1. (we can discuss this aspect later)

  9. When within a certain distance of WP1, switch to WP2.

  10. When within a certain distance of WP2, switch to WP0 (home).

  11. When within a certain distance of WP0, switch to manual mode and drive a compass heading (to guarantee you make it back to shore).

  12. Operator will shut down the unit when it gets back to shore.

If these are the steps, we can discuss the details. Steps 5, 6, and navigating are the hard nuts. Taking one at a time should be your focus.

Just for kicks, have you looked at the RC plane autopilot project I have seen on the web someplace?

13

To compute WP1, you will need WP0, azimuth (az), and range.

I am assuming your GPS is outputing data in DDDMMSS.ss and in ASCII.

You will need to convert an ASCII fix position you get from the GPS from a string like N42 45 12 W78 30 34 into two numbers something like +42.75 and -30.5. So now you can save these values as you home point and the center of a local coordinate system.

You you should know the azimuth as well. Convert to radians (deg * 3.1415926/180.0). The Northing distance you want to travel will be cos(azRad)*rangeInMeters. The Easting distance will be sin(azRad)*rangeInMeters.

So WP1 equals WP0 plus the vector Range<azimuth. Breaking this down into north and east parts we get

WP1 Northing = WP0 Northing + cos(azRad)*rangeInMeters

WP1 Easting = WP0 Easting + sin(azRad)*rangeInMeters

So what is WP0/1 Northing? Well since each degree of latitude is 185260 meters long, you can multiply your latitude in decimal degrees by 185260 and get your Northing.

The same does not hold true for Easting. Degrees of logitude do not have the same length as they are pie shaped towards the poles. So you must account for this by adjusting with the cosine of the latitude.

Easting = decialLong * 1852 / 60 / cos(lat).

So… You convert your WP0 into Northing0 and Easting0. You add your vector Northing and Easting to get the Northing and Easting of WP1. Now you can get the decimal values for WP1 lat/long by using the above equations in reverse.

So now you should have WP1 (or WP2 for that mater) in decimal degrees. As you motor your boat, you will get the ASCII position from the GPS and convert it to decimal degrees. This you can compare to where you want to go.

What I do not know is if any of the Arduino libraries provide the ASCII to decimal convertion or the decimal to Northing/Easting conversions.

14

Thanks for all that.

I will need a few weeks to digest it and try to work up a sketch.

I will let you know how it goes.

15

Did you make any progress on this?