rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

OP-1474 Make it possible to select the combination of P, I, and D to scale

Browse Source
This commit is contained in:
Stefan Karlsson 2014-09-19 00:26:09 +02:00
parent d3e7ef300f
commit 31c5f1c519
9 changed files with 100 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/libraries/math/mathmisc.h
View File

@ -88,7 +88,7 @@ typedef struct pointf {
} pointf;
// Returns the y value, given x, on the line passing through the points p0 and p1.
static inline float y_on_line(float x, pointf *p0, pointf *p1)
static inline float y_on_line(float x, const pointf *p0, const pointf *p1)
{
// Setup line y = m * x + b.
const float dY1 = p1->y - p0->y;
@ -102,7 +102,7 @@ static inline float y_on_line(float x, pointf *p0, pointf *p1)
// Returns the y value, given x, on the curve defined by the points array.
// The fist and last line of the curve extends beyond the first resp. last points.
static inline float y_on_curve(float x, pointf points[], int num_points)
static inline float y_on_curve(float x, const pointf points[], int num_points)
{
// Find the two points x is within.
// If x is smaller than the first point's x value, use the first line of the curve.

15
flight/libraries/math/pid.c
View File

@ -76,12 +76,12 @@ float pid_apply(struct pid *pid, const float err, float dT)
* This version of apply uses setpoint weighting for the derivative component so the gain
* on the gyro derivative can be different than the gain on the setpoint derivative
*/
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT)
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT)
{
float err = setpoint - measured;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
pid->iAccumulator += err * (scaler->i * pid->i * dT * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
@ -89,11 +89,11 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if (pid->d > 0.0f && dT > 0.0f) {
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer);
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm;
return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}
/**
@ -140,10 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
float pid_scale_factor(pid_scaler *scaler)
{
float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
return 1.0f + (IS_REAL(y) ? y : 0.0f);
}

8
flight/libraries/math/pid.h
View File

@ -45,16 +45,16 @@ struct pid {
};
typedef struct pid_scaler {
float x;
pointf points[5];
float p;
float i;
float d;
} pid_scaler;
// ! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT);
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);
float pid_scale_factor(pid_scaler *scaler);
#endif /* PID_H */

8
flight/modules/Stabilization/altitudeloop.c
View File

@ -164,7 +164,9 @@ static void altitudeHoldTask(void)
switch (thrustMode) {
case ALTITUDEHOLD:
// altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
// No scaling.
pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
@ -182,7 +184,9 @@ static void altitudeHoldTask(void)
break;
default:
// velocity control loop
thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
// No scaling.
pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
break;
}

102
flight/modules/Stabilization/innerloop.c
View File

@ -123,29 +123,90 @@ static float get_pid_scale_source_value()
return value;
}
static float get_pid_scale_factor()
{
struct pid_scaler scaler = {
.x = get_pid_scale_source_value(),
.points = {
{ 0.0f, stabSettings.stabBank.ThrustPIDScaleCurve[0] },
{ 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] },
{ 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] },
{ 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] },
{ 1.0f, stabSettings.stabBank.ThrustPIDScaleCurve[4] }
}
};
return pid_scale_factor(&scaler);
}
static int is_pid_scaled_for_axis(int axis)
static int is_pid_thrust_scaled_for_axis(int axis)
{
return stabSettings.stabBank.EnableThrustPIDScaling
&& (axis == 0 // Roll
|| axis == 1); // Pitch
}
static bool is_p_scaling_enabled()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
}
static bool is_i_scaling_enabled()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
}
static bool is_d_scaling_enabled()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
}
typedef struct pid_curve_scaler {
float x;
pointf points[5];
} pid_curve_scaler;
static float pid_curve_value(const pid_curve_scaler *scaler)
{
float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
return 1.0f + (IS_REAL(y) ? y : 0.0f);
}
static pid_scaler create_pid_scaler(int axis)
{
pid_scaler scaler;
// Always scaled with the this.
scaler.p = scaler.i = scaler.d = speedScaleFactor;
if (is_pid_thrust_scaled_for_axis(axis)) {
const pid_curve_scaler curve_scaler = {
.x = get_pid_scale_source_value(),
.points = {
{ 0.00f, stabSettings.stabBank.ThrustPIDScaleCurve[0] },
{ 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] },
{ 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] },
{ 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] },
{ 1.00f, stabSettings.stabBank.ThrustPIDScaleCurve[4] }
}
};
float curve_value = pid_curve_value(&curve_scaler);
if (is_p_scaling_enabled()) {
scaler.p *= curve_value;
}
if (is_i_scaling_enabled()) {
scaler.i *= curve_value;
}
if (is_d_scaling_enabled()) {
scaler.d *= curve_value;
}
}
return scaler;
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
@ -252,11 +313,8 @@ static void stabilizationInnerloopTask()
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
float scaleFactor = speedScaleFactor;
if (is_pid_scaled_for_axis(t)) {
scaleFactor *= get_pid_scale_factor();
}
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], scaleFactor, rate[t], gyro_filtered[t], dT);
pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:

1
shared/uavobjectdefinition/stabilizationbank.xml
View File

@ -18,6 +18,7 @@
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

1
shared/uavobjectdefinition/stabilizationsettingsbank1.xml
View File

@ -18,6 +18,7 @@
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

1
shared/uavobjectdefinition/stabilizationsettingsbank2.xml
View File

@ -18,6 +18,7 @@
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

1
shared/uavobjectdefinition/stabilizationsettingsbank3.xml
View File

@ -18,6 +18,7 @@
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>