# Special Request for a new "get_bearing" function in ArduPilot source

I'd like to ask any of the math wizards out there if you can write and post an improved "get_bearing" function for the navigation tab of the ArduPilot source.

The more I test the ArduPilot code with my emulator, the more nervous I get. You'll notice in the screenshot of my 43MPH flight around Paris on my emulator, the ArduPilot decided to take a few loops around waypoints and the larger the distance between the waypoints, the greater the error in bearing.

Here's the existing get_bearing function

long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}

Views: 248

3D Robotics
Comment by Chris Anderson on May 22, 2010 at 10:05pm
Have you tried just increasing the crosstrack error correction?

//4-4
#define XTRACK_GAIN 00 // amount to compensate for crosstrack (degrees/100 per meter)
//4-5
#define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100

Developer
Comment by Ryan Beall on May 22, 2010 at 10:18pm
This is the equation for initial course from wpt1 to wpt2:

lamda = atan2(cos(lat1*(PI/180))*sin(lat2*(PI/180))-sin(lat1*(PI/180))*cos(lat2*(PI/180))*cos((lon2-lon1)*(PI/180)), sin((lon2-lon1)*(PI/180))*cos(lat2*(PI/180)));
lamda = -(lamda-(PI/2));

Developer
Comment by Ryan Beall on May 22, 2010 at 10:22pm
I don't know exactly what you are doing but it probably isn't the accuracy of the course as much as is it may be the heading controller. Do you model gps latency? If so the controller as written has room for a lot of improvement.

Moderator
Comment by Alex on May 22, 2010 at 11:24pm
Are you sure its a problem with the ' get_bearing' function? and not something with simulator? Because most of us seem to fly just fine the way it is. I understand the current get bearing function is not the most accurate but it is good enough for distances less that 10km

Alternatively what ryan posted, but in an easier format

θ = atan2(sin(Δlong).cos(lat2), cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong) )

Δlat = lat2− lat1
Δlong = long2− long1
Comment by Jeff Taylor on May 22, 2010 at 11:29pm
you might be less nervous if you were emulating a different locale! : )

T3
Comment by Sami Finnila on May 23, 2010 at 4:13am
You could also introduce a "line hold" function; it'd make achieving more complex navigation routines easier. I could help with it since I've already written such a algorithm and tested that it's working. However, I think that Chris is right; the problem is probably not in the bearing function but the coefficients or something with the simulation.
Comment by Paul Mather on May 23, 2010 at 5:50am
I'll try your ideas guys. There is always the possibility that the problem is with my emulator, I'll give you that. However, trying to reason this out, I'm showing exactly where my emulator is telling the ArduPilot to go. If the ArduPilot didn't like my heading (because it was calculated wrong or whatever) you'd see the ArduPilot fighting as hard as it could to aim towards the next point. This is not the case. between points, the ArduPilot is oscillating slightly but generally on a 0 nav_roll. It has to make a sharp right to hit the waypoint and then yanks hard left on it's way to the next waypoint.
Comment by Patrick Mccabe on May 23, 2010 at 8:38am
Here is my bearing code for my AGV. All done in radians.
x2lat = my waypoint latitude
x2lon= my waypoint longitude
flat1 = current latitude from the gps
flon1 = current longitude from the gps

}

Comment by Paul Mather on May 23, 2010 at 8:54am
Patrick, I had downloaded your code from your AGV and was trying last night to get it working. I'm not sure if the stored data on the ArduPilot is in degrees or radians and I'm not 100% on the output format either (but it seems like it's degrees * 100)... so I'm keeping my fingers crossed your code will produce a better result. Thanks!

Developer
Comment by Jason Short on May 23, 2010 at 11:25am
Hi Happy,
I was curious to after seeing that behavior. It does appear to be wrong, but the equation is fine. I think you have the wrong scaler input. There is a value called scaleLongUp which is based on the latitude. If that value were 1, you would see the errors you are getting.

I just ran the numbers with these points near the Eifel Tower and got proper results. Ardupilot output 44.68 vs 44.65 in Google Earth.

Here is the file I used to create and test the method. Mainly I was looking for something as fast as possible and accurate out a few miles only. (20 or so is fairly accurate) If someone wants to replace it, just place the alternate in this file and run the tests for speed and accuracy in various latitudes.

bearing_alt2.zip

Jason

Comment

Join DIY Drones

1

2

3

4

5

6

7

8

9

10

## Groups

1791 members

192 members

83 members

273 members

• ### Image Orthorectification

74 members

Season Two of the Trust Time Trial (T3) Contest has now begun. The fifth round, 3D model building requiring contestants to survey and reconstruct a building or natural scene from aerial images.

A list of all T3 contests is here.