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.

http://diydrones.com/profiles/blogs/improved-heading-controller

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! : )

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


heading = atan2(sin(x2lon-flon1)*cos(x2lat),cos(flat1)*sin(x2lat)-sin(flat1)*cos(x2lat)*cos(x2lon-flon1)),2*3.1415926535;
heading = heading*180/3.1415926535; // convert from radians to degrees
int head =heading;
if(head<0){
heading+=360; //if the heading is negative then add 360 to make it positive
}

Serial.println("heading:");
Serial.println(heading);

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 jasonshort 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

• ‹ Previous
• 1
• 2
• 3
• Next ›
• Page