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

PathPlanner: added functions to allow circular paths and implement that feature in FixedWingPathFollower

Browse Source
This commit is contained in:
Corvus Corax 2012-05-26 15:55:52 +02:00
parent 45258a6561
commit 9dfd34dd19
3 changed files with 96 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/Libraries/inc/paths.h
View File

@ -35,5 +35,6 @@ struct path_status {
};
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status);
void circle_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
#endif

56
flight/Libraries/paths.c
View File

@ -78,3 +78,59 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
status->error = fabs(status->error);
}
/**
* @brief Compute progress along circular path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Center point
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
void circle_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
{
float radius_north, radius_east, diff_north, diff_east;
float radius;
float normal[2];
// Radius
radius_north = end_point[0] - start_point[0];
radius_east = end_point[1] - start_point[1];
// Current location relative to center
diff_north = cur_point[0] - end_point[0];
diff_east = cur_point[1] - end_point[1];
radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
if(radius < 1e-3) {
status->fractional_progress = 1;
status->error = 0;
status->correction_direction[0] = status->correction_direction[1] = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
if (clockwise) {
// Compute the normal to the radius clockwise
normal[0] = radius_east / radius;
normal[1] = -radius_north / radius;
} else {
// Compute the normal to the radius counter clockwise
normal[0] = -radius_east / radius;
normal[1] = radius_north / radius;
}
status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
// error is current radius minus wanted radius - positive if too close
status->error = radius - sqrtf( diff_north * diff_north + diff_east * diff_east );
// Compute direction to correct error
status->correction_direction[0] = -status->error * radius_north / radius;
status->correction_direction[1] = -status->error * radius_east / radius;
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->error = fabs(status->error);
}

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

@ -86,7 +86,7 @@ static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
// Private functions
static void pathfollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent * ev);
static void updatePathVelocity();
static void updatePathVelocity(int8_t circledirection);
static void updateEndpointVelocity();
static void updateFixedDesiredAttitude();
static void updateFixedAttitude();
@ -140,7 +140,6 @@ static float courseIntegral = 0;
static float speedIntegral = 0;
static float accelIntegral = 0;
static float powerIntegral = 0;
static uint8_t positionHoldLast = 0;
// correct speed by measured airspeed
static float baroAirspeedBias = 0;
@ -212,7 +211,17 @@ static void pathfollowerTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FLYVECTOR:
updatePathVelocity();
updatePathVelocity(0);
updateFixedDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
updatePathVelocity(1);
updateFixedDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity(-1);
updateFixedDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
@ -250,7 +259,7 @@ static void pathfollowerTask(void *parameters)
* Takes in @ref PositionActual and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
static void updatePathVelocity(int8_t circledirection)
{
//float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand;
@ -260,36 +269,44 @@ static void updatePathVelocity()
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
if (!circledirection) {
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
} else {
circle_progress(pathDesired.Start, pathDesired.End, cur, &progress, circledirection>0 );
}
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
float groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
if(progress.fractional_progress > 1)
float groundspeed;
if (!circledirection && progress.fractional_progress<1) {
groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
} else {
groundspeed = pathDesired.EndingVelocity;
}
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
// prevent div by zero
if (fabsf(correction_velocity[0])+fabsf(correction_velocity[1]) <1e-6) {
correction_velocity[0]=1e-6;
}
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
velocityDesired.North += progress.correction_direction[0] * error_speed;
velocityDesired.East += progress.correction_direction[1] * error_speed;
float total_vel = sqrtf(powf(velocityDesired.North,2) + powf(velocityDesired.East,2));
float scale = 1;
if(total_vel > fixedwingpathfollowerSettings.HorizontalVelMax)
if(total_vel > fixedwingpathfollowerSettings.HorizontalVelMax) {
scale = fixedwingpathfollowerSettings.HorizontalVelMax / total_vel;
if (total_vel < fixedwingpathfollowerSettings.HorizontalVelMin)
} else if (total_vel < fixedwingpathfollowerSettings.HorizontalVelMin && total_vel>1e-6) {
scale = fixedwingpathfollowerSettings.HorizontalVelMin / total_vel;
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
} else {
/* if we are not supposed to move, head north with minimum velocity - to prevent errors */
scale = 1;
velocityDesired.North = fixedwingpathfollowerSettings.HorizontalVelMin;
velocityDesired.East = 0;
}
velocityDesired.North*= scale;
velocityDesired.East *= scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);