rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Further cleanup in PathFollower. Should now be safe to fly again

Browse Source
This commit is contained in:
Corvus Corax 2012-10-28 21:49:22 +01:00
parent f7dd5cc8ad
commit 6409dfb2df
3 changed files with 34 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -282,7 +282,6 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
// groundspeed = fixedwingpathfollowerSettings.BestClimbRateSpeed;
altitudeSetpoint = pathDesired.End[2];
break;
case PATHDESIRED_MODE_FLYENDPOINT:
@ -291,16 +290,19 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1);
bound(progress.fractional_progress,0,1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
bound(progress.fractional_progress,0,1);
break;
}
// this ensures a significant forward component at least close to the real trajectory
if (groundspeed<fixedwingpathfollowerSettings.BestClimbRateSpeed/10.)
groundspeed=fixedwingpathfollowerSettings.BestClimbRateSpeed/10.;
// calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * fmaxf(groundspeed,1e-6);
velocityDesired.East = progress.path_direction[1] * fmaxf(groundspeed,1e-6);
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
@ -366,10 +368,6 @@ static uint8_t updateFixedDesiredAttitude()
float indicatedAirspeedActual;
float indicatedAirspeedDesired;
float airspeedError;
float accelActual;
float accelDesired;
float accelError;
float accelCommand;
float pitchCommand;
@ -522,66 +520,28 @@ static uint8_t updateFixedDesiredAttitude()
/**
* Compute desired pitch command
*/
// compute desired acceleration
if(0){
accelDesired = bound( (airspeedError/indicatedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = 0.0f;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = accelDesired;
// exclude gravity from acceleration. If the aicraft is flying at high pitch, it fights gravity without getting faster
accelActual = accels.x - (sinf( DEG2RAD * attitudeActual.Pitch) * GEE);
accelError = accelDesired - accelActual;
if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
accelIntegral = bound(accelIntegral + accelError * dT,
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
}
accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] +
accelIntegral*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){
//Integrate with saturation
airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
}
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = accelError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = accelIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = accelCommand;
pitchCommand= -accelCommand + bound ( (-descentspeedError/indicatedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
}
else {
if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
//Integrate with saturation
airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
}
//Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP]
+ airspeedErrorInt*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = indicatedAirspeedDesired;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = -123;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = -123;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = pitchCommand;
}
//Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP]
+ airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
pitchCommand,

11
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -6,9 +6,9 @@
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="20"/>
<!-- Vne, i.e. maximum airspeed the airframe can handle -->
<field name="CruiseSpeed" units="m/s" type="float" elements="1" defaultvalue="20"/>
<!-- Cruise speed the in level flight - leave some safety margin -->
<!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
<field name="BestClimbRateSpeed" units="m/s" type="float" elements="1" defaultvalue="10"/>
<!-- V_y, i.e. airspeed at which plane climbs the best -->
<!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
<field name="StallSpeedClean" units="m/s" type="float" elements="1" defaultvalue="8"/>
<!-- Vs1, i.e. stall speed in clean configuration- leave some safety margin -->
<field name="StallSpeedDirty" units="m/s" type="float" elements="1" defaultvalue="8"/>
@ -28,12 +28,9 @@
<field name="BearingPI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
<!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading -->
<field name="SpeedP" units="(m/s^2) / ((m/s)/(m/s)" type="float" elementnames="Kp,Max" defaultvalue="10,10"/>
<!-- coefficients for desired acceleration
in relation to relative airspeed error (IASerror/IASactual)-->
<field name="AccelPI" units="deg / (m/s^2)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
<field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
<!-- coefficients for desired pitch
in relation to acceleration error -->
in relation to relative speed error (IASerror/IASactual) -->
<field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
<!-- coefficients for adjusting desired pitch
in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->

6
shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
<description>Object Storing Debugging Information on PID internals</description>
<field name="Error" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
<field name="Command" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
<field name="Error" units="" type="float" elementnames="Bearing,Speed,Power" />
<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Power" />
<field name="Command" units="" type="float" elementnames="Bearing,Speed,Power" />
<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>