mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +01:00
Flight/Stabilization: Make the derivative account for the sampling time (dAngle/dt) for the PID loop since integral does and also make the integral bounds uavobject settings. It was originally bounded by constant/Ki but I'm not sure why as you change the gain of the error integral you also want to decrease the maximum error it can cause.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1590 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
8999355327
commit
c376711405
@ -44,9 +44,6 @@
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define PITCH_INTEGRAL_LIMIT 0.5
|
||||
#define ROLL_INTEGRAL_LIMIT 0.5
|
||||
#define YAW_INTEGRAL_LIMIT 0.5
|
||||
|
||||
// Private types
|
||||
|
||||
@ -88,9 +85,9 @@ static void stabilizationTask(void* parameters)
|
||||
float pitchDerivative;
|
||||
float rollDerivative;
|
||||
float yawDerivative;
|
||||
float pitchIntegral, pitchIntegralLimit;
|
||||
float rollIntegral, rollIntegralLimit;
|
||||
float yawIntegral, yawIntegralLimit;
|
||||
float pitchIntegral;
|
||||
float rollIntegral;
|
||||
float yawIntegral;
|
||||
|
||||
// Initialize
|
||||
pitchIntegral = 0.0;
|
||||
@ -113,18 +110,16 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
// Pitch stabilization control loop
|
||||
pitchError = bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch;
|
||||
pitchDerivative = pitchError - pitchErrorLast;
|
||||
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
|
||||
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
|
||||
pitchDerivative = (pitchError - pitchErrorLast) / stabSettings.UpdatePeriod;
|
||||
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -stabSettings.PitchIntegralLimit, stabSettings.PitchIntegralLimit);
|
||||
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
|
||||
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
|
||||
pitchErrorLast = pitchError;
|
||||
|
||||
// Roll stabilization control loop
|
||||
rollError = bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll;
|
||||
rollDerivative = rollError - rollErrorLast;
|
||||
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
|
||||
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
|
||||
rollDerivative = (rollError - rollErrorLast) / stabSettings.UpdatePeriod;
|
||||
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -stabSettings.RollIntegralLimit, stabSettings.RollIntegralLimit);
|
||||
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
|
||||
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
|
||||
rollErrorLast = rollError;
|
||||
@ -136,9 +131,8 @@ static void stabilizationTask(void* parameters)
|
||||
//this should make it take the quickest path to reach the desired yaw
|
||||
if (yawError>180.0)yawError -= 360;
|
||||
if (yawError<-180.0)yawError += 360;
|
||||
yawDerivative = yawError - yawErrorLast;
|
||||
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
|
||||
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
|
||||
yawDerivative = (yawError - yawErrorLast) / stabSettings.UpdatePeriod;
|
||||
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -stabSettings.YawIntegralLimit, stabSettings.YawIntegralLimit);
|
||||
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
|
||||
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
|
||||
yawErrorLast = yawError;
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
#define STABILIZATIONSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define STABILIZATIONSETTINGS_OBJID 2185681924U
|
||||
#define STABILIZATIONSETTINGS_OBJID 3082215042U
|
||||
#define STABILIZATIONSETTINGS_NAME "StabilizationSettings"
|
||||
#define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta"
|
||||
#define STABILIZATIONSETTINGS_ISSINGLEINST 1
|
||||
@ -75,6 +75,9 @@ typedef struct {
|
||||
float RollMax;
|
||||
float PitchMax;
|
||||
float ThrottleMax;
|
||||
float RollIntegralLimit;
|
||||
float PitchIntegralLimit;
|
||||
float YawIntegralLimit;
|
||||
float PitchKp;
|
||||
float PitchKi;
|
||||
float PitchKd;
|
||||
@ -92,6 +95,9 @@ typedef struct {
|
||||
// Field RollMax information
|
||||
// Field PitchMax information
|
||||
// Field ThrottleMax information
|
||||
// Field RollIntegralLimit information
|
||||
// Field PitchIntegralLimit information
|
||||
// Field YawIntegralLimit information
|
||||
// Field PitchKp information
|
||||
// Field PitchKi information
|
||||
// Field PitchKd information
|
||||
|
||||
@ -84,6 +84,9 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
data.RollMax = 35;
|
||||
data.PitchMax = 35;
|
||||
data.ThrottleMax = 1;
|
||||
data.RollIntegralLimit = 0.5;
|
||||
data.PitchIntegralLimit = 0.5;
|
||||
data.YawIntegralLimit = 0.5;
|
||||
data.PitchKp = 0.04;
|
||||
data.PitchKi = 4e-06;
|
||||
data.PitchKd = 0.01;
|
||||
|
||||
@ -54,6 +54,15 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS
|
||||
QStringList ThrottleMaxElemNames;
|
||||
ThrottleMaxElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("ThrottleMax"), QString("%"), UAVObjectField::FLOAT32, ThrottleMaxElemNames, QStringList()) );
|
||||
QStringList RollIntegralLimitElemNames;
|
||||
RollIntegralLimitElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("RollIntegralLimit"), QString(""), UAVObjectField::FLOAT32, RollIntegralLimitElemNames, QStringList()) );
|
||||
QStringList PitchIntegralLimitElemNames;
|
||||
PitchIntegralLimitElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("PitchIntegralLimit"), QString(""), UAVObjectField::FLOAT32, PitchIntegralLimitElemNames, QStringList()) );
|
||||
QStringList YawIntegralLimitElemNames;
|
||||
YawIntegralLimitElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("YawIntegralLimit"), QString(""), UAVObjectField::FLOAT32, YawIntegralLimitElemNames, QStringList()) );
|
||||
QStringList PitchKpElemNames;
|
||||
PitchKpElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("PitchKp"), QString(""), UAVObjectField::FLOAT32, PitchKpElemNames, QStringList()) );
|
||||
@ -118,6 +127,9 @@ void StabilizationSettings::setDefaultFieldValues()
|
||||
data.RollMax = 35;
|
||||
data.PitchMax = 35;
|
||||
data.ThrottleMax = 1;
|
||||
data.RollIntegralLimit = 0.5;
|
||||
data.PitchIntegralLimit = 0.5;
|
||||
data.YawIntegralLimit = 0.5;
|
||||
data.PitchKp = 0.04;
|
||||
data.PitchKi = 4e-06;
|
||||
data.PitchKd = 0.01;
|
||||
|
||||
@ -47,6 +47,9 @@ public:
|
||||
float RollMax;
|
||||
float PitchMax;
|
||||
float ThrottleMax;
|
||||
float RollIntegralLimit;
|
||||
float PitchIntegralLimit;
|
||||
float YawIntegralLimit;
|
||||
float PitchKp;
|
||||
float PitchKi;
|
||||
float PitchKd;
|
||||
@ -64,6 +67,9 @@ public:
|
||||
// Field RollMax information
|
||||
// Field PitchMax information
|
||||
// Field ThrottleMax information
|
||||
// Field RollIntegralLimit information
|
||||
// Field PitchIntegralLimit information
|
||||
// Field YawIntegralLimit information
|
||||
// Field PitchKp information
|
||||
// Field PitchKi information
|
||||
// Field PitchKd information
|
||||
@ -76,7 +82,7 @@ public:
|
||||
|
||||
|
||||
// Constants
|
||||
static const quint32 OBJID = 2185681924U;
|
||||
static const quint32 OBJID = 3082215042U;
|
||||
static const QString NAME;
|
||||
static const bool ISSINGLEINST = 1;
|
||||
static const bool ISSETTINGS = 1;
|
||||
|
||||
@ -77,6 +77,36 @@ _fields = [ \
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'RollIntegralLimit',
|
||||
'f',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'PitchIntegralLimit',
|
||||
'f',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'YawIntegralLimit',
|
||||
'f',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'PitchKp',
|
||||
'f',
|
||||
@ -172,7 +202,7 @@ _fields = [ \
|
||||
|
||||
class StabilizationSettings(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 2185681924
|
||||
OBJID = 3082215042
|
||||
NAME = "StabilizationSettings"
|
||||
METANAME = "StabilizationSettingsMeta"
|
||||
ISSINGLEINST = 1
|
||||
|
||||
@ -5,6 +5,9 @@
|
||||
<field name="RollMax" units="degrees" type="float" elements="1" defaultvalue="35"/>
|
||||
<field name="PitchMax" units="degrees" type="float" elements="1" defaultvalue="35"/>
|
||||
<field name="ThrottleMax" units="%" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="RollIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="PitchIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="YawIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="PitchKp" units="" type="float" elements="1" defaultvalue="0.04"/>
|
||||
<field name="PitchKi" units="" type="float" elements="1" defaultvalue="0.000004"/>
|
||||
<field name="PitchKd" units="" type="float" elements="1" defaultvalue="0.01"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user