rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +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 { struct path_status {
float fractional_progress; float fractional_progress;
float error; float error;
float correction_direction[2]; float correction_direction[3];
float path_direction[2]; float path_direction[3];
}; };
void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); 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 // no direct UAVObject usage allowed in this file
// private functions // 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_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); 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; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
return path_endpoint(start_point, end_point, cur_point, status, true);
break;
case PATHDESIRED_MODE_DRIVEENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT:
default: default:
// use the endpoint as default failsafe if called in unknown modes // 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; 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] end_point Ending point
* @param[in] cur_point Current location * @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation * @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; float dist_path, dist_diff;
// we do not correct in this mode // 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 // Distance to go
path_north = end_point[0] - start_point[0]; path[0] = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1]; path[1] = end_point[1] - start_point[1];
path[2] = mode3D ? end_point[2] - start_point[2] : 0;
// Current progress location relative to end // Current progress location relative to end
diff_north = end_point[0] - cur_point[0]; diff[0] = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1]; 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_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]);
dist_path = sqrtf(path_north * path_north + path_east * path_east); dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]);
if (dist_diff < 1e-6f) { if (dist_diff < 1e-6f) {
status->fractional_progress = 1; status->fractional_progress = 1;
status->error = 0; 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; return;
} }
@ -111,8 +117,9 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point
status->error = dist_diff; status->error = dist_diff;
// Compute direction to travel // Compute direction to travel
status->path_direction[0] = diff_north / dist_diff; status->path_direction[0] = diff[0] / dist_diff;
status->path_direction[1] = diff_east / 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. // if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away, // Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way. // 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; status->fractional_progress = 1;
return; return;
} }
@ -159,6 +166,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
// Compute direction to correct error // Compute direction to correct error
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; 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[1] = (status->error > 0) ? -normal[1] : normal[1];
status->correction_direction[2] = 0;
// Now just want magnitude of error // Now just want magnitude of error
status->error = fabs(status->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 // Compute direction to travel
status->path_direction[0] = path_north / dist_path; status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / 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->error = radius;
status->correction_direction[0] = 0; status->correction_direction[0] = 0;
status->correction_direction[1] = 1; status->correction_direction[1] = 1;
status->correction_direction[2] = 0;
status->path_direction[0] = 1; status->path_direction[0] = 1;
status->path_direction[1] = 0; status->path_direction[1] = 0;
status->path_direction[2] = 0;
return; return;
} }
@ -246,10 +257,12 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
// Compute direction to correct error // Compute direction to correct error
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; 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[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
status->correction_direction[2] = 0;
// Compute direction to travel // Compute direction to travel
status->path_direction[0] = normal[0]; status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1]; status->path_direction[1] = normal[1];
status->path_direction[2] = 0;
status->error = fabs(status->error); 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), cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode); cur, &progress, pathDesired.Mode);
float groundspeed; float speed;
switch (pathDesired.Mode) { switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity; speed = pathDesired.EndingVelocity;
break; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT: 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) { if (progress.fractional_progress > 1) {
groundspeed = 0; speed = 0;
} }
break; break;
case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR:
default: default:
groundspeed = pathDesired.StartingVelocity speed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) { if (progress.fractional_progress > 1) {
groundspeed = 0; speed = 0;
} }
break; break;
} }
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.North = progress.path_direction[0] * speed;
velocityDesired.East = progress.path_direction[1] * groundspeed; velocityDesired.East = progress.path_direction[1] * speed;
velocityDesired.Down = progress.path_direction[2] * speed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
float correction_velocity[2] = float correction_velocity[2] =
@ -436,7 +437,7 @@ static void updatePathVelocity()
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit, -vtolpathfollowerSettings.VerticalPosPI.ILimit,
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); velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus // update pathstatus