mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
FixedWingPathFollower: Redesigned and tested FixedWingpathFollower
This commit is contained in:
parent
ef8d7275a8
commit
634a190d4f
@ -74,6 +74,7 @@
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
|
||||
#define F_PI 3.14159265358979323846f
|
||||
#define RAD2DEG (180.0f/F_PI)
|
||||
#define DEG2RAD (F_PI/180.0f)
|
||||
#define GEE 9.81f
|
||||
// Private types
|
||||
|
||||
@ -289,8 +290,14 @@ static void updatePathVelocity(int8_t circledirection)
|
||||
{
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
|
||||
VelocityActualData velocityActual;
|
||||
VelocityActualGet(&velocityActual);
|
||||
|
||||
// look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds
|
||||
float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.CourseFeedForward),
|
||||
positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.CourseFeedForward),
|
||||
positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.CourseFeedForward)
|
||||
};
|
||||
struct path_status progress;
|
||||
|
||||
if (!circledirection) {
|
||||
@ -340,23 +347,25 @@ static void updatePathVelocity(int8_t circledirection)
|
||||
void updateEndpointVelocity()
|
||||
{
|
||||
PositionActualData positionActual;
|
||||
VelocityActualData velocityActual;
|
||||
VelocityDesiredData velocityDesired;
|
||||
|
||||
PositionActualGet(&positionActual);
|
||||
VelocityActualGet(&velocityActual);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
|
||||
float northError;
|
||||
float eastError;
|
||||
float downError;
|
||||
|
||||
// Compute commands
|
||||
northError = pathDesired.End[PATHDESIRED_END_NORTH] - positionActual.North;
|
||||
// Compute commands - look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds
|
||||
northError = pathDesired.End[PATHDESIRED_END_NORTH] - ( positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.CourseFeedForward));
|
||||
velocityDesired.North = northError * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
|
||||
eastError = pathDesired.End[PATHDESIRED_END_EAST] - positionActual.East;
|
||||
eastError = pathDesired.End[PATHDESIRED_END_EAST] - ( positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.CourseFeedForward));
|
||||
velocityDesired.East = eastError * fixedwingpathfollowerSettings.HorizontalPosP;
|
||||
|
||||
downError = pathDesired.End[PATHDESIRED_END_DOWN] - positionActual.Down;
|
||||
downError = pathDesired.End[PATHDESIRED_END_DOWN] - ( positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.CourseFeedForward));
|
||||
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
@ -408,6 +417,7 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
float airspeedActual;
|
||||
float airspeedDesired;
|
||||
float speedError;
|
||||
float accelActual;
|
||||
float accelDesired;
|
||||
float accelError;
|
||||
float accelCommand;
|
||||
@ -451,7 +461,6 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
|
||||
// Airspeed error
|
||||
speedError = airspeedDesired - ( airspeedActual );
|
||||
|
||||
// Vertical error
|
||||
climbspeedDesired = bound (
|
||||
velocityDesired.Down,
|
||||
@ -464,19 +473,28 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
result = 0;
|
||||
}
|
||||
// Error condition: plane too slow or too fast
|
||||
if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.1f
|
||||
|| airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.9f
|
||||
if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.2f
|
||||
|| airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f
|
||||
) {
|
||||
result = 0;
|
||||
}
|
||||
|
||||
|
||||
if (airspeedActual<1e-6) {
|
||||
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
|
||||
// also we cannot handle planes flying backwards, lets just wait until the nose drops
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired throttle command
|
||||
*/
|
||||
// compute desired power
|
||||
powerError = speedError * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeedP - climbspeedError;
|
||||
powerError = -climbspeedError +
|
||||
bound (
|
||||
(speedError/airspeedActual) * fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_KP],
|
||||
-fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX],
|
||||
fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX]
|
||||
);
|
||||
powerIntegral = bound(powerIntegral + powerError * dT * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
|
||||
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
|
||||
@ -510,13 +528,6 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
// set throttle
|
||||
stabDesired.Throttle = powerCommand;
|
||||
|
||||
if(fixedwingpathfollowerSettings.ThrottleControl == FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
|
||||
// Manual throttle control, warning: will not hold altitude correctly without that
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
stabDesired.Throttle = manualControl.Throttle;
|
||||
}
|
||||
|
||||
// Error condition: plane cannot hold altitude at current speed.
|
||||
if (
|
||||
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
|
||||
@ -541,15 +552,18 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
* Compute desired pitch command
|
||||
*/
|
||||
// compute desired acceleration
|
||||
accelDesired = bound( speedError * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
|
||||
accelDesired = bound( (speedError/airspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
|
||||
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
|
||||
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
|
||||
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = speedError;
|
||||
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = (speedError/airspeedActual);
|
||||
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_SPEED] = 0.0f;
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_SPEED] = accelDesired;
|
||||
|
||||
accelError = accelDesired - accels.x;
|
||||
|
||||
// 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;
|
||||
accelIntegral = bound(accelIntegral + accelError * dT * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
|
||||
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT],
|
||||
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]);
|
||||
@ -561,7 +575,10 @@ static uint8_t updateFixedDesiredAttitude()
|
||||
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_ACCEL] = accelCommand;
|
||||
|
||||
// compute desired pitch
|
||||
pitchCommand= -accelCommand + fixedwingpathfollowerSettings.VerticalToPitchCrossFeedP * -climbspeedError;
|
||||
pitchCommand= -accelCommand + bound ( (-climbspeedError/airspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
|
||||
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
|
||||
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
|
||||
);
|
||||
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
|
||||
pitchCommand,
|
||||
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
|
||||
@ -658,9 +675,9 @@ static void baroAirspeedUpdatedCb(UAVObjEvent * ev)
|
||||
baroAirspeedBias = 0;
|
||||
} else {
|
||||
VelocityActualGet(&velocityActual);
|
||||
float speed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North + velocityActual.Down*velocityActual.Down );
|
||||
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
|
||||
|
||||
baroAirspeedBias = baroAirspeed.Airspeed - speed;
|
||||
baroAirspeedBias = baroAirspeed.Airspeed - groundspeed;
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -1,21 +1,51 @@
|
||||
<xml>
|
||||
<object name="FixedWingPathFollowerSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref FixedWingPathFollowerModule</description>
|
||||
|
||||
<!-- these coefficients control desired movement in airspace -->
|
||||
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="15"/>
|
||||
<!-- maximum speed the aircraft can reach in level flight at full throttle without loosing altitude - leave some safety margin -->
|
||||
<field name="AirSpeedMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="2"/>
|
||||
<!-- stall speed - leave some safety margin -->
|
||||
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10"/>
|
||||
<!-- maximum allowed climb or sink rate in guided flight-->
|
||||
|
||||
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
|
||||
<!-- how many seconds to plan the flight vector ahead when initiating necessary course changes - increase for planes with sluggish response -->
|
||||
|
||||
<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="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"/>
|
||||
<!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
|
||||
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
|
||||
<!-- proportional coefficient for desired vertical speed in relation to altitude displacement-->
|
||||
|
||||
<!-- these coefficients control actual control outputs -->
|
||||
<field name="CoursePI" 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 heading 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, 1.5, 20"/>
|
||||
<!-- coefficients for desired pitch
|
||||
in relation to acceleration error -->
|
||||
<field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="50, 5"/>
|
||||
<!-- coefficients for adjusting desired pitch
|
||||
in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->
|
||||
<field name="AirspeedToVerticalCrossFeed" units="(m/s) / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="100, 100"/>
|
||||
<!-- proportional coefficient for adjusting vertical speed error for power calculation
|
||||
in relation to relative airspeed error (IASerror/IASactual) -->
|
||||
<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
|
||||
in relation to vertical speed error (absolute but including crossfeed) -->
|
||||
|
||||
<!-- output limits -->
|
||||
<field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-35,0,35" />
|
||||
<!-- maximum allowed bank angles in navigates flight -->
|
||||
<field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" />
|
||||
<!-- maximum allowed pitch angles and setpoint for neutral pitch -->
|
||||
<field name="ThrottleLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
|
||||
<!-- minimum and maximum allowed throttle and setpoint for cruise speed -->
|
||||
|
||||
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user