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

changed some details as to dschin's suggestions

Browse Source
This commit is contained in:
Corvus Corax 2012-11-11 16:35:19 +01:00
parent 264f631df6
commit 7059d4f032
2 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -138,7 +138,6 @@ static float downVelIntegral = 0;
static float bearingIntegral = 0; static float bearingIntegral = 0;
static float speedIntegral = 0; static float speedIntegral = 0;
static float accelIntegral = 0;
static float powerIntegral = 0; static float powerIntegral = 0;
static float airspeedErrorInt=0; static float airspeedErrorInt=0;
@ -242,7 +241,6 @@ static void pathfollowerTask(void *parameters)
downVelIntegral = 0; downVelIntegral = 0;
bearingIntegral = 0; bearingIntegral = 0;
speedIntegral = 0; speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0; powerIntegral = 0;
break; break;
@ -294,20 +292,24 @@ static void updatePathVelocity()
bound(progress.fractional_progress,0,1); bound(progress.fractional_progress,0,1);
break; break;
} }
// this ensures a significant forward component at least close to the real trajectory // make sure groundspeed is not zero
if (groundspeed<fixedwingpathfollowerSettings.BestClimbRateSpeed/10.) if (groundspeed<1e-6) groundspeed=1e-6;
groundspeed=fixedwingpathfollowerSettings.BestClimbRateSpeed/10.;
// calculate velocity - can be zero if waypoints are too close // calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.North = progress.path_direction[0];
velocityDesired.East = progress.path_direction[1] * groundspeed; velocityDesired.East = progress.path_direction[1];
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
// calculate correction - can also be zero if correction vector is 0 or no error present // calculate correction - can also be zero if correction vector is 0 or no error present
velocityDesired.North += progress.correction_direction[0] * error_speed; velocityDesired.North += progress.correction_direction[0] * error_speed;
velocityDesired.East += progress.correction_direction[1] * error_speed; velocityDesired.East += progress.correction_direction[1] * error_speed;
//scale to correct length
float l=sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
velocityDesired.North *= groundspeed/l;
velocityDesired.East *= groundspeed/l;
float downError = altitudeSetpoint - positionActual.Down; float downError = altitudeSetpoint - positionActual.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
@ -372,7 +374,6 @@ static uint8_t updateFixedDesiredAttitude()
float descentspeedDesired; float descentspeedDesired;
float descentspeedError; float descentspeedError;
float powerError;
float powerCommand; float powerCommand;
float bearingError; float bearingError;
@ -458,14 +459,6 @@ static uint8_t updateFixedDesiredAttitude()
/** /**
* Compute desired throttle command * Compute desired throttle command
*/ */
// compute proportional throttle response
powerError = -descentspeedError +
bound (
(airspeedError/fixedwingpathfollowerSettings.BestClimbRateSpeed)* fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_KP] ,
-fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX],
fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX]
);
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) { if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT, powerIntegral = bound(powerIntegral + -descentspeedError * dT,
@ -474,20 +467,27 @@ static uint8_t updateFixedDesiredAttitude()
)*(1.0f-1.0f/(1.0f+30.0f/dT)); )*(1.0f-1.0f/(1.0f+30.0f/dT));
} else powerIntegral = 0; } else powerIntegral = 0;
//Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound (
(airspeedError/fixedwingpathfollowerSettings.BestClimbRateSpeed)* fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP] ,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX],
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX]
);
// Compute final throttle response // Compute final throttle response
powerCommand = bound( powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
(powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] +
powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL], speedErrorToPowerCommandComponent;
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
//Output internal state to telemetry //Output internal state to telemetry
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = powerError; fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand;
// set throttle // set throttle
stabDesired.Throttle = powerCommand; stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
// Error condition: plane cannot hold altitude at current speed. // Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;

10
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -30,13 +30,13 @@
<field name="SpeedPI" units="deg / (m/s)" 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 <!-- coefficients for desired pitch
in relation to relative speed error (IASerror/IASactual) --> in relation to speed error IASerror -->
<field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/> <field name="VerticalToPitchCrossFeed" units="deg / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
<!-- coefficients for adjusting desired pitch <!-- coefficients for adjusting desired pitch
in relation to "vertical speed error relative to airspeed" (verror/IASactual) --> in relation to "vertical speed error -->
<field name="AirspeedToVerticalCrossFeed" units="(m/s) / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="10, 100"/> <field name="AirspeedToPowerCrossFeed" units="1 / (m/s)" type="float" elementnames="Kp,Max" defaultvalue="0.1, 1"/>
<!-- proportional coefficient for adjusting vertical speed error for power calculation <!-- proportional coefficient for adjusting vertical speed error for power calculation
in relation to relative airspeed error (IASerror/IASactual) --> in relation to airspeed error IASerror -->
<field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.01,0.8"/> <field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.01,0.8"/>
<!-- proportional coefficient for desired throttle <!-- proportional coefficient for desired throttle
in relation to vertical speed error (absolute but including crossfeed) --> in relation to vertical speed error (absolute but including crossfeed) -->