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

Add a 3D mode to path_endpoint. This allows FlyDirect waypoints to do

Browse Source 
vertical movements without changing position.
This commit is contained in:
Werner Backes 2014-07-15 09:30:24 +02:00
parent 33439a8781
commit 3e2961420d
3 changed files with 40 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/libraries/inc/paths.h
View File

@ -30,8 +30,8 @@
struct path_status {
float fractional_progress;
float error;
float correction_direction[2];
float path_direction[2];
float correction_direction[3];
float path_direction[3];
};
void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode);

43
flight/libraries/paths.c
View File

@ -34,7 +34,7 @@
// no direct UAVObject usage allowed in this file
// private functions
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status);
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode);
static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status);
static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise);
@ -65,10 +65,13 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
break;
case PATHDESIRED_MODE_FLYENDPOINT:
return path_endpoint(start_point, end_point, cur_point, status, true);
break;
case PATHDESIRED_MODE_DRIVEENDPOINT:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status);
return path_endpoint(start_point, end_point, cur_point, status, false);
break;
}
@ -80,30 +83,33 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
* @param[in] end_point Ending point
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
* @param[in] mode3D set true to include altitude in distance and progress calculation
*/
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status)
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D)
{
float path_north, path_east, diff_north, diff_east;
float path[3], diff[3];
float dist_path, dist_diff;
// we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0;
status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0;
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
path[0] = end_point[0] - start_point[0];
path[1] = end_point[1] - start_point[1];
path[2] = mode3D ? end_point[2] - start_point[2] : 0;
// Current progress location relative to end
diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1];
diff[0] = end_point[0] - cur_point[0];
diff[1] = end_point[1] - cur_point[1];
diff[2] = mode3D ? end_point[2] - cur_point[2] : 0;
dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
dist_path = sqrtf(path_north * path_north + path_east * path_east);
dist_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]);
dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]);
if (dist_diff < 1e-6f) {
status->fractional_progress = 1;
status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0;
status->path_direction[0] = status->path_direction[1] = status->path_direction[2] = 0;
return;
}
@ -111,8 +117,9 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point
status->error = dist_diff;
// Compute direction to travel
status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff;
status->path_direction[0] = diff[0] / dist_diff;
status->path_direction[1] = diff[1] / dist_diff;
status->path_direction[2] = diff[2] / dist_diff;
}
/**
@ -144,7 +151,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
// if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint(start_point, end_point, cur_point, status);
path_endpoint(start_point, end_point, cur_point, status, false);
status->fractional_progress = 1;
return;
}
@ -159,6 +166,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
status->correction_direction[2] = 0;
// Now just want magnitude of error
status->error = fabs(status->error);
@ -166,6 +174,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
// Compute direction to travel
status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / dist_path;
status->path_direction[2] = 0;
}
/**
@ -200,8 +209,10 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
status->error = radius;
status->correction_direction[0] = 0;
status->correction_direction[1] = 1;
status->correction_direction[2] = 0;
status->path_direction[0] = 1;
status->path_direction[1] = 0;
status->path_direction[2] = 0;
return;
}
@ -246,10 +257,12 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
// Compute direction to correct error
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
status->correction_direction[2] = 0;
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->path_direction[2] = 0;
status->error = fabs(status->error);
}

19
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -387,35 +387,36 @@ static void updatePathVelocity()
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
float groundspeed;
float speed;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
speed = pathDesired.EndingVelocity;
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
speed = 0;
}
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
speed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
speed = 0;
}
break;
}
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
velocityDesired.North = progress.path_direction[0] * speed;
velocityDesired.East = progress.path_direction[1] * speed;
velocityDesired.Down = progress.path_direction[2] * speed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
float correction_velocity[2] =
@ -436,7 +437,7 @@ static void updatePathVelocity()
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
downCommand = velocityDesired.Down + (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus