aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: don't do crosstrack in last 3m

Browse Source 
this reduces the weaving at waypoints
This commit is contained in:
Andrew Tridgell 2013-05-17 08:33:02 +10:00
parent ca8e4519cc
commit b5f2b4db8a
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/navigation.pde
View File

@ -50,7 +50,9 @@ static void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
// If we are too far off or too close we don't do track following
if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500 && wp_distance >= 3.0f) {
crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain_float(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360_cd(nav_bearing);