XTRACK_GAIN = .2if (abs(bearing_error) < 25) { bearing_error += sin(radians(crosstrack_heading - bearing_error)) * wp_distance * XTRACK_GAIN;}We set the crosstrack from our starting location as the angle toward the waypoint. As we get blown from side to side, that angle changes. We run that through sine to get a nice ratio and multiply it to get an angle to add to our heading.Depending on the gain and bearing error, we can get wild results, so we need to limit the effect to a range of degrees. In this case 25°. If we go outside of 25° by a lot, we can reset the crosstrack to be from the current location.Here is an example:
You need to be a member of diydrones to add comments!
Comments
I'm glad you put in some edits. I had a constraint in my code as well, but must not have snipped enough. Rather than ignore the crosstrack error when it gets too large, I just constrain it to a maximum. The result is pretty similar to Ryan's vector field. We should take a look and see which is computationally lighter :)
I might need a bit more detail on your two calls get_lambda and cross_track to understand what your doing. I like it though!
In this version, I throw out anything too wacky. You can see the green bar go transparent.
Maybe this pic will help describe what I'm getting at
entry_angle = 65*(PI/180); //enters track at 65 deg from 1 going to 2
track_gain = 0.0001;
lamda = get_lamda(lat1,lon1,lat2,lon2);
y_track = cross_track(lat1,lon1,lat2,lon2,lat3,lon3);
psi_cmd = (-entry_angle*(2/PI)*atan(track_gain*-y_track))+lamda;
psi_cmd = wrap(psi_cmd)*(180/PI);