Skip to content

Commit

Permalink
fixed yawDiff bug in autopilot
Browse files Browse the repository at this point in the history
JakobEngel committed Apr 26, 2013
1 parent 3e74cb1 commit 03bd100
Showing 1 changed file with 2 additions and 2 deletions.
4 changes: 2 additions & 2 deletions src/autopilot/KI/KIFlyTo.cpp
Original file line number Diff line number Diff line change
@@ -84,15 +84,15 @@ bool KIFlyTo::update(const tum_ardrone::filter_stateConstPtr statePtr)
double diffDistSquared = diffs[0] * diffs[0] + diffs[1] * diffs[1] + diffs[2] * diffs[2];

// if not reached yet, need to get within small radius to count.
if(!reached && diffDistSquared < initialReachedDist * initialReachedDist && diffYaw < 5)
if(!reached && diffDistSquared < initialReachedDist * initialReachedDist && diffYaw*diffYaw < 25)
{
reached = true;
reachedAtClock = getMS();
printf("target reached initially!\n");
}

// if too far away again: revoke reached status...
if(reached && (diffDistSquared > stayWithinDist * stayWithinDist || diffYaw > 5))
if(reached && (diffDistSquared > stayWithinDist * stayWithinDist || diffYaw*diffYaw > 25))
{
reached = false;
printf("target lost again!\n");

0 comments on commit 03bd100

Please sign in to comment.