mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Add a 3D mode to path_endpoint. This allows FlyDirect waypoints to do
vertical movements without changing position.
This commit is contained in:
parent
33439a8781
commit
3e2961420d
@ -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);
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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
|
||||
|
Loading…
x
Reference in New Issue
Block a user