rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

Cheat. For FW navigation when doing endpoint mode plot path from current

Browse Source 
position to endpoint.
This commit is contained in:
James Cotton 2012-06-08 11:44:22 -05:00
parent 980111d246
commit ccee8e09f4
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -245,20 +245,26 @@ static void updatePathVelocity()
VelocityActualGet(&velocityActual); VelocityActualGet(&velocityActual);
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, &positionActual.North, &progress);
float groundspeed; float groundspeed;
float altitudeSetpoint; float altitudeSetpoint;
switch (pathDesired.Mode) { switch (pathDesired.Mode) {
case PATHDESIRED_MODE_PATH: case PATHDESIRED_MODE_PATH:
case PATHDESIRED_MODE_ENDPOINT: path_progress(pathDesired.Start, pathDesired.End, &positionActual.North, &progress);
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1); bound(progress.fractional_progress,0,1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1); bound(progress.fractional_progress,0,1);
break; 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 // calculate velocity - can be zero if waypoints are too close