mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
PathFollower and PathPlanner: fixes for fixed wing
This commit is contained in:
Corvus Corax
parent
922b3b7f1e
commit
ef8d7275a8
@ -37,7 +37,7 @@
|
||||
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status)
|
||||
{
|
||||
float path_north, path_east, diff_north, diff_east;
|
||||
float dist_path2;
|
||||
float dist_path;
|
||||
float dot;
|
||||
float normal[2];
|
||||
|
||||
@ -50,30 +50,30 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
|
||||
diff_east = cur_point[1] - start_point[1];
|
||||
|
||||
dot = path_north * diff_north + path_east * diff_east;
|
||||
dist_path2 = path_north * path_north + path_east * path_east;
|
||||
dist_path = sqrtf( path_north * path_north + path_east * path_east );
|
||||
|
||||
if(dist_path2 < 1e-3) {
|
||||
if(dist_path < 1e-6) {
|
||||
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;
|
||||
status->path_direction[0] = status->path_direction[1] = 1e-6;
|
||||
return;
|
||||
}
|
||||
|
||||
// Compute the normal to the path
|
||||
normal[0] = -path_east / sqrtf(dist_path2);
|
||||
normal[1] = path_north / sqrtf(dist_path2);
|
||||
normal[0] = -path_east / dist_path;
|
||||
normal[1] = path_north / dist_path;
|
||||
|
||||
status->fractional_progress = dot / dist_path2;
|
||||
status->fractional_progress = dot / (dist_path * dist_path);
|
||||
status->error = normal[0] * diff_north + normal[1] * diff_east;
|
||||
|
||||
// Compute direction to correct error
|
||||
status->correction_direction[0] = -status->error * normal[0];
|
||||
status->correction_direction[1] = -status->error * normal[1];
|
||||
status->correction_direction[0] = (status->error>0?1:-1) * -normal[0];
|
||||
status->correction_direction[1] = (status->error>0?1:-1) * -normal[1];
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = path_north / sqrtf(dist_path2);
|
||||
status->path_direction[1] = path_east / sqrtf(dist_path2);
|
||||
status->path_direction[0] = path_north / dist_path;
|
||||
status->path_direction[1] = path_east / dist_path;
|
||||
|
||||
status->error = fabs(status->error);
|
||||
}
|
||||
@ -88,7 +88,7 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
|
||||
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 radius,cradius;
|
||||
float normal[2];
|
||||
|
||||
// Radius
|
||||
@ -100,33 +100,37 @@ void circle_progress(float * start_point, float * end_point, float * cur_point,
|
||||
diff_east = cur_point[1] - end_point[1];
|
||||
|
||||
radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
|
||||
cradius = sqrtf( diff_north * diff_north + diff_east * diff_east );
|
||||
|
||||
if(radius < 1e-3) {
|
||||
if (cradius < 1e-6) {
|
||||
// cradius is zero, just fly somewhere and make sure correction is still a normal
|
||||
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;
|
||||
status->error = radius;
|
||||
status->correction_direction[0] = 0;
|
||||
status->correction_direction[1] = 1;
|
||||
status->path_direction[0] = 1;
|
||||
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;
|
||||
normal[0] = -diff_east / cradius;
|
||||
normal[1] = diff_north / cradius;
|
||||
} else {
|
||||
// Compute the normal to the radius counter clockwise
|
||||
normal[0] = -radius_east / radius;
|
||||
normal[1] = radius_north / radius;
|
||||
normal[0] = diff_east / cradius;
|
||||
normal[1] = -diff_north / cradius;
|
||||
}
|
||||
|
||||
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 );
|
||||
status->error = radius - cradius;
|
||||
|
||||
// Compute direction to correct error
|
||||
status->correction_direction[0] = -status->error * radius_north / radius;
|
||||
status->correction_direction[1] = -status->error * radius_east / radius;
|
||||
status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
|
||||
status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = normal[0];
|
||||
|
||||
@ -88,7 +88,7 @@ static void pathfollowerTask(void *parameters);
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev);
|
||||
static void updatePathVelocity(int8_t circledirection);
|
||||
static void updateEndpointVelocity();
|
||||
static void updateFixedDesiredAttitude();
|
||||
static uint8_t updateFixedDesiredAttitude();
|
||||
static void updateFixedAttitude();
|
||||
static void baroAirspeedUpdatedCb(UAVObjEvent * ev);
|
||||
static float bound(float val, float min, float max);
|
||||
@ -187,15 +187,20 @@ static void pathfollowerTask(void *parameters)
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
PathStatusGet(&pathStatus);
|
||||
|
||||
|
||||
uint8_t result;
|
||||
// Check the combinations of flightmode and pathdesired mode
|
||||
switch(flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
|
||||
updateEndpointVelocity();
|
||||
updateFixedDesiredAttitude();
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
result = updateFixedDesiredAttitude();
|
||||
if (result) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
@ -207,23 +212,43 @@ static void pathfollowerTask(void *parameters)
|
||||
// TODO: Make updateFixedDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
updateEndpointVelocity();
|
||||
updateFixedDesiredAttitude();
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
result = updateFixedDesiredAttitude();
|
||||
if (result) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
updatePathVelocity(0);
|
||||
updateFixedDesiredAttitude();
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
result = updateFixedDesiredAttitude();
|
||||
if (result) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
updatePathVelocity(1);
|
||||
updateFixedDesiredAttitude();
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
result = updateFixedDesiredAttitude();
|
||||
if (result) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
updatePathVelocity(-1);
|
||||
updateFixedDesiredAttitude();
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
result = updateFixedDesiredAttitude();
|
||||
if (result) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
break;
|
||||
case PATHDESIRED_MODE_FIXEDATTITUDE:
|
||||
updateFixedAttitude(pathDesired.ModeParameters);
|
||||
@ -250,6 +275,7 @@ static void pathfollowerTask(void *parameters)
|
||||
|
||||
break;
|
||||
}
|
||||
PathStatusSet(&pathStatus);
|
||||
}
|
||||
}
|
||||
|
||||
@ -261,9 +287,6 @@ static void pathfollowerTask(void *parameters)
|
||||
*/
|
||||
static void updatePathVelocity(int8_t circledirection)
|
||||
{
|
||||
//float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
float downCommand;
|
||||
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
@ -278,8 +301,8 @@ static void updatePathVelocity(int8_t circledirection)
|
||||
|
||||
float groundspeed;
|
||||
if (!circledirection && progress.fractional_progress<1) {
|
||||
groundspeed = pathDesired.StartingVelocity +
|
||||
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
|
||||
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
} else {
|
||||
groundspeed = pathDesired.EndingVelocity;
|
||||
}
|
||||
@ -289,33 +312,21 @@ static void updatePathVelocity(int8_t circledirection)
|
||||
velocityDesired.East = progress.path_direction[1] * groundspeed;
|
||||
|
||||
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
|
||||
|
||||
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) {
|
||||
scale = fixedwingpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
} else if (total_vel < fixedwingpathfollowerSettings.HorizontalVelMin && total_vel>1e-6) {
|
||||
scale = fixedwingpathfollowerSettings.HorizontalVelMin / total_vel;
|
||||
} 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]) *
|
||||
float altitudeSetpoint;
|
||||
if (!circledirection) {
|
||||
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
} else {
|
||||
altitudeSetpoint = pathDesired.End[2];
|
||||
}
|
||||
|
||||
float downError = altitudeSetpoint - positionActual.Down;
|
||||
downCommand = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
velocityDesired.Down = bound(downCommand,
|
||||
-fixedwingpathfollowerSettings.VerticalVelMax,
|
||||
fixedwingpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -328,8 +339,6 @@ static void updatePathVelocity(int8_t circledirection)
|
||||
*/
|
||||
void updateEndpointVelocity()
|
||||
{
|
||||
//float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
PositionActualData positionActual;
|
||||
VelocityDesiredData velocityDesired;
|
||||
|
||||
@ -339,38 +348,16 @@ void updateEndpointVelocity()
|
||||
float northError;
|
||||
float eastError;
|
||||
float downError;
|
||||
float northCommand;
|
||||
float eastCommand;
|
||||
float downCommand;
|
||||
|
||||
// Compute commands
|
||||
northError = pathDesired.End[PATHDESIRED_END_NORTH] - positionActual.North;
|
||||
northCommand = northError * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
velocityDesired.North = northError * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
|
||||
eastError = pathDesired.End[PATHDESIRED_END_EAST] - positionActual.East;
|
||||
eastCommand = eastError * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
velocityDesired.East = eastError * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
|
||||
// prevent div by zero
|
||||
if (fabsf(northCommand)+fabsf(eastCommand) <1e-6) {
|
||||
northCommand=1e-6;
|
||||
}
|
||||
|
||||
// Limit the maximum velocity
|
||||
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
|
||||
float scale = 1;
|
||||
if(total_vel > fixedwingpathfollowerSettings.HorizontalVelMax)
|
||||
scale = fixedwingpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
if (total_vel < fixedwingpathfollowerSettings.HorizontalVelMin)
|
||||
scale = fixedwingpathfollowerSettings.HorizontalVelMin / total_vel;
|
||||
|
||||
velocityDesired.North = northCommand * scale;
|
||||
velocityDesired.East = eastCommand * scale;
|
||||
|
||||
downError = pathDesired.End[PATHDESIRED_END_DOWN] - positionActual.Down;
|
||||
downCommand = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
velocityDesired.Down = bound(downCommand,
|
||||
-fixedwingpathfollowerSettings.VerticalVelMax,
|
||||
fixedwingpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -400,8 +387,11 @@ static void updateFixedAttitude(float* attitude)
|
||||
* NED frame as the feedback term and then compares the
|
||||
* @ref VelocityActual against the @ref VelocityDesired
|
||||
*/
|
||||
static void updateFixedDesiredAttitude()
|
||||
static uint8_t updateFixedDesiredAttitude()
|
||||
{
|
||||
|
||||
uint8_t result = 1;
|
||||
|
||||
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
@ -413,20 +403,25 @@ static void updateFixedDesiredAttitude()
|
||||
StabilizationSettingsData stabSettings;
|
||||
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
|
||||
|
||||
float courseError;
|
||||
float courseCommand;
|
||||
|
||||
float groundspeedActual;
|
||||
float groundspeedDesired;
|
||||
float airspeedActual;
|
||||
float airspeedDesired;
|
||||
float speedError;
|
||||
float accelCommand;
|
||||
|
||||
float speedActual;
|
||||
float speedDesired;
|
||||
float accelDesired;
|
||||
float accelError;
|
||||
float accelCommand;
|
||||
|
||||
float pitchCommand;
|
||||
|
||||
float climbspeedDesired;
|
||||
float climbspeedError;
|
||||
float powerError;
|
||||
float powerCommand;
|
||||
|
||||
float courseError;
|
||||
float courseCommand;
|
||||
|
||||
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
|
||||
|
||||
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
|
||||
@ -439,63 +434,49 @@ static void updateFixedDesiredAttitude()
|
||||
AccelsGet(&accels);
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
// current speed - lacking forward airspeed we use groundspeed :(
|
||||
speedActual = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North + velocityActual.Down*velocityActual.Down ) + baroAirspeedBias;
|
||||
|
||||
// Compute desired roll command
|
||||
courseError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
|
||||
if (courseError<-180.0f) courseError+=360.0f;
|
||||
if (courseError>180.0f) courseError-=360.0f;
|
||||
/**
|
||||
* Compute speed error (required for throttle and pitch)
|
||||
*/
|
||||
|
||||
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI],
|
||||
-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]);
|
||||
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] +
|
||||
courseIntegral);
|
||||
// Current ground speed
|
||||
groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
|
||||
airspeedActual = groundspeedActual + baroAirspeedBias;
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_COURSE] = courseError;
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_COURSE] = courseIntegral;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_COURSE] = courseCommand;
|
||||
|
||||
stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
|
||||
courseCommand,
|
||||
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] );
|
||||
// Desired ground speed
|
||||
groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
|
||||
airspeedDesired = bound( groundspeedDesired + baroAirspeedBias,
|
||||
fixedwingpathfollowerSettings.AirSpeedMin,
|
||||
fixedwingpathfollowerSettings.AirSpeedMax);
|
||||
|
||||
// Compute desired yaw command
|
||||
// TODO implement raw control mode for yaw and base on Accels.X
|
||||
stabDesired.Yaw = 0;
|
||||
// Airspeed error
|
||||
speedError = airspeedDesired - ( airspeedActual );
|
||||
|
||||
// Compute desired speed command TODO: find out wind vector and compensate
|
||||
speedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
|
||||
speedError = speedDesired - speedActual;
|
||||
// Vertical error
|
||||
climbspeedDesired = bound (
|
||||
velocityDesired.Down,
|
||||
-fixedwingpathfollowerSettings.VerticalVelMax,
|
||||
fixedwingpathfollowerSettings.VerticalVelMax);
|
||||
climbspeedError = climbspeedDesired - velocityActual.Down;
|
||||
|
||||
accelDesired = bound( speedError * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
|
||||
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
|
||||
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = speedError;
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_SPEED] = 0.0f;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_SPEED] = accelDesired;
|
||||
|
||||
accelError = accelDesired - accels.x;
|
||||
accelIntegral = bound(accelIntegral + accelError * dT * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
|
||||
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]);
|
||||
accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] +
|
||||
accelIntegral);
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_ACCEL] = accelError;
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_ACCEL] = accelIntegral;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_ACCEL] = accelCommand;
|
||||
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
|
||||
if (groundspeedDesired - baroAirspeedBias <= 0 ) {
|
||||
result = 0;
|
||||
}
|
||||
// Error condition: plane too slow or too fast
|
||||
if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.1f
|
||||
|| airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.9f
|
||||
) {
|
||||
result = 0;
|
||||
}
|
||||
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
|
||||
-accelCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
|
||||
|
||||
// Compute desired power command
|
||||
powerError = -( velocityDesired.Down - velocityActual.Down ) * fixedwingpathfollowerSettings.ClimbRateBoostFactor + speedError;
|
||||
|
||||
/**
|
||||
* Compute desired throttle command
|
||||
*/
|
||||
// compute desired power
|
||||
powerError = speedError * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeedP - climbspeedError;
|
||||
powerIntegral = bound(powerIntegral + powerError * dT * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
|
||||
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
|
||||
@ -530,12 +511,110 @@ static void updateFixedDesiredAttitude()
|
||||
stabDesired.Throttle = powerCommand;
|
||||
|
||||
if(fixedwingpathfollowerSettings.ThrottleControl == FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
|
||||
// For now override throttle with manual control. Disable at your risk, quad goes to China.
|
||||
// Manual throttle control, warning: will not hold altitude correctly without that
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
stabDesired.Throttle = manualControl.Throttle;
|
||||
}
|
||||
//printf("Cycle: speed Error: %f\n powerError: %f\n accelCommand: %f\n powerCommand: %f\n\n",speedError,powerError,accelCommand,powerCommand);
|
||||
|
||||
// Error condition: plane cannot hold altitude at current speed.
|
||||
if (
|
||||
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
|
||||
&& velocityActual.Down > 0 // we ARE going down
|
||||
&& climbspeedDesired < 0 // we WANT to go up
|
||||
&& speedError > 0 // we are too slow already
|
||||
) {
|
||||
result = 0;
|
||||
}
|
||||
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
|
||||
if (
|
||||
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
|
||||
&& velocityActual.Down < 0 // we ARE going up
|
||||
&& climbspeedDesired > 0 // we WANT to go down
|
||||
&& speedError < 0 // we are too fast already
|
||||
) {
|
||||
result = 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired pitch command
|
||||
*/
|
||||
// compute desired acceleration
|
||||
accelDesired = bound( speedError * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
|
||||
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
|
||||
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = speedError;
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_SPEED] = 0.0f;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_SPEED] = accelDesired;
|
||||
|
||||
accelError = accelDesired - accels.x;
|
||||
accelIntegral = bound(accelIntegral + accelError * dT * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
|
||||
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]);
|
||||
accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] +
|
||||
accelIntegral);
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_ACCEL] = accelError;
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_ACCEL] = accelIntegral;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_ACCEL] = accelCommand;
|
||||
|
||||
// compute desired pitch
|
||||
pitchCommand= -accelCommand + fixedwingpathfollowerSettings.VerticalToPitchCrossFeedP * -climbspeedError;
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
|
||||
pitchCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
|
||||
|
||||
// Error condition: high speed dive
|
||||
if (
|
||||
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
|
||||
&& velocityActual.Down > 0 // we ARE going down
|
||||
&& climbspeedDesired < 0 // we WANT to go up
|
||||
&& speedError < 0 // we are too fast already
|
||||
) {
|
||||
result = 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired roll command
|
||||
*/
|
||||
if (groundspeedDesired> 1e-6) {
|
||||
courseError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
|
||||
} else {
|
||||
// if we are not supposed to move, keep going wherever we are now. Don't make things worse by changing direction.
|
||||
courseError = 0;
|
||||
}
|
||||
|
||||
if (courseError<-180.0f) courseError+=360.0f;
|
||||
if (courseError>180.0f) courseError-=360.0f;
|
||||
|
||||
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI],
|
||||
-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]);
|
||||
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] +
|
||||
courseIntegral);
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_COURSE] = courseError;
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_COURSE] = courseIntegral;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_COURSE] = courseCommand;
|
||||
|
||||
stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
|
||||
courseCommand,
|
||||
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
|
||||
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] );
|
||||
|
||||
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired yaw command
|
||||
*/
|
||||
// TODO implement raw control mode for yaw and base on Accels.X
|
||||
stabDesired.Yaw = 0;
|
||||
|
||||
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
@ -544,6 +623,8 @@ static void updateFixedDesiredAttitude()
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
|
||||
FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -478,6 +478,10 @@ static uint8_t conditionImmediate() {
|
||||
static void createPathBox()
|
||||
{
|
||||
|
||||
uint16_t t;
|
||||
for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
|
||||
PathActionCreateInstance();
|
||||
}
|
||||
PathActionData action;
|
||||
PathActionInstGet(0,&action);
|
||||
action.Mode = PATHACTION_MODE_FLYVECTOR;
|
||||
@ -494,6 +498,8 @@ static void createPathBox()
|
||||
action.JumpDestination = 0;
|
||||
action.ErrorDestination = 0;
|
||||
PathActionInstSet(0,&action);
|
||||
PathActionInstSet(1,&action);
|
||||
PathActionInstSet(2,&action);
|
||||
|
||||
WaypointCreateInstances(6);
|
||||
|
||||
@ -531,6 +537,7 @@ static void createPathBox()
|
||||
// demo path - logo
|
||||
static void createPathLogo()
|
||||
{
|
||||
#define SIZE 10.0f
|
||||
PathActionData action;
|
||||
PathActionInstGet(0,&action);
|
||||
action.Mode = PATHACTION_MODE_FLYVECTOR;
|
||||
@ -547,6 +554,10 @@ static void createPathLogo()
|
||||
action.JumpDestination = 0;
|
||||
action.ErrorDestination = 0;
|
||||
PathActionInstSet(0,&action);
|
||||
uint16_t t;
|
||||
for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
|
||||
PathActionCreateInstance();
|
||||
}
|
||||
|
||||
WaypointCreateInstances(42);
|
||||
|
||||
@ -555,53 +566,53 @@ static void createPathLogo()
|
||||
waypoint.Velocity = 2; // Since for now this isn't directional just set a mag
|
||||
waypoint.Action = 0;
|
||||
for(uint32_t i = 0; i < 20; i++) {
|
||||
waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
|
||||
waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
|
||||
waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI);
|
||||
waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI);
|
||||
waypoint.Position[2] = -50;
|
||||
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
|
||||
}
|
||||
|
||||
// Draw P
|
||||
for(uint32_t i = 20; i < 35; i++) {
|
||||
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
|
||||
waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
|
||||
waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2));
|
||||
waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2));
|
||||
waypoint.Position[2] = -50;
|
||||
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
|
||||
}
|
||||
|
||||
waypoint.Position[1] = 35;
|
||||
waypoint.Position[0] = -50;
|
||||
waypoint.Position[1] = SIZE * 35;
|
||||
waypoint.Position[0] = SIZE * -50;
|
||||
waypoint.Position[2] = -50;
|
||||
WaypointInstSet(35, &waypoint);
|
||||
|
||||
// Draw Box
|
||||
waypoint.Position[1] = 35;
|
||||
waypoint.Position[0] = -60;
|
||||
waypoint.Position[1] = SIZE * 35;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(36, &waypoint);
|
||||
|
||||
waypoint.Position[1] = 85;
|
||||
waypoint.Position[0] = -60;
|
||||
waypoint.Position[1] = SIZE * 85;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(37, &waypoint);
|
||||
|
||||
waypoint.Position[1] = 85;
|
||||
waypoint.Position[0] = 60;
|
||||
waypoint.Position[1] = SIZE * 85;
|
||||
waypoint.Position[0] = SIZE * 60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(38, &waypoint);
|
||||
|
||||
waypoint.Position[1] = -40;
|
||||
waypoint.Position[0] = 60;
|
||||
waypoint.Position[1] = SIZE * -40;
|
||||
waypoint.Position[0] = SIZE * 60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(39, &waypoint);
|
||||
|
||||
waypoint.Position[1] = -40;
|
||||
waypoint.Position[0] = -60;
|
||||
waypoint.Position[1] = SIZE * -40;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(40, &waypoint);
|
||||
|
||||
waypoint.Position[1] = 35;
|
||||
waypoint.Position[0] = -60;
|
||||
waypoint.Position[1] = SIZE * 35;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(41, &waypoint);
|
||||
|
||||
|
||||
@ -1,15 +1,16 @@
|
||||
<xml>
|
||||
<object name="FixedWingPathFollowerSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref FixedWingPathFollowerModule</description>
|
||||
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="15"/>
|
||||
<field name="HorizontalVelMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
|
||||
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="15"/>
|
||||
<field name="AirSpeedMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="2"/>
|
||||
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
|
||||
<field name="VerticalPosP" units="m/s" type="float" elements="1" defaultvalue="0.05"/>
|
||||
<field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2,0.02,10"/>
|
||||
<field name="SpeedP" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Max" defaultvalue="10,10"/>
|
||||
<field name="AccelPI" units="deg/(m/s^2)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.1,10"/>
|
||||
<field name="ClimbRateBoostFactor" units="" type="float" elements="1" defaultvalue="3.14"/>
|
||||
<field name="VerticalToPitchCrossFeedP" units="" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="AirspeedToPowerCrossFeedP" units="" type="float" elements="1" defaultvalue="0.32"/>
|
||||
<field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.005,0.005,0.8"/>
|
||||
<field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
|
||||
<field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-35,0,35" />
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user