From ccee8e09f43cf7f427ee5cb96cfbead37835db94 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 8 Jun 2012 11:44:22 -0500 Subject: [PATCH] Cheat. For FW navigation when doing endpoint mode plot path from current position to endpoint. --- .../FixedWingPathFollower/fixedwingpathfollower.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c index 9b7904a69..8b700d128 100644 --- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -245,20 +245,26 @@ static void updatePathVelocity() VelocityActualGet(&velocityActual); struct path_status progress; - - path_progress(pathDesired.Start, pathDesired.End, &positionActual.North, &progress); float groundspeed; float altitudeSetpoint; switch (pathDesired.Mode) { case PATHDESIRED_MODE_PATH: - case PATHDESIRED_MODE_ENDPOINT: - default: + path_progress(pathDesired.Start, pathDesired.End, &positionActual.North, &progress); groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress,0,1); altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress,0,1); break; + case PATHDESIRED_MODE_ENDPOINT: + path_progress(&positionActual.North, pathDesired.End, &positionActual.North, &progress); + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + bound(progress.fractional_progress,0,1); + altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * + bound(progress.fractional_progress,0,1); + break; + default: + break; } // calculate velocity - can be zero if waypoints are too close