rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-76 easier to understand yaw PID limiting

Browse Source
This commit is contained in:
Cliff Geerdes 2016-03-12 18:19:09 -05:00
parent a217d0015f
commit b48ed29fc5
2 changed files with 50 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
flight/modules/AutoTune/autotune.c
View File

@ -668,7 +668,7 @@ static uint8_t CheckSettingsRaw()
if (systemIdentState.Beta.Pitch < 6) {
retVal |= PITCH_BETA_LOW;
}
if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) {
if (systemIdentState.Beta.Yaw < 4) {
retVal |= YAW_BETA_LOW;
}
// Check the response speed
@ -693,9 +693,10 @@ static uint8_t CheckSettings()
{
uint8_t retVal = CheckSettingsRaw();
if (systemIdentSettings.DisableSanityChecks
|| ((retVal == YAW_BETA_LOW) && (!systemIdentSettings.CalculateYaw || systemIdentSettings.OverrideYawBeta))) {
retVal = 0;
if (systemIdentSettings.DisableSanityChecks) {
retVal = 0;
} else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) {
retVal &= ~YAW_BETA_LOW;
}
return retVal;
@ -758,19 +759,46 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn);
// dRonin simply uses default PID settings for yaw
for (int i = 0; i < ((systemIdentSettings.CalculateYaw) ? 3 : 2); i++) {
double beta;
// if yaw axis and yaw beta is too low and user wants to override it if too low
if (i == 2 && systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin && systemIdentSettings.OverrideYawBeta) {
beta = exp(systemIdentSettings.YawBetaMin);
} else {
beta = exp(SystemIdentStateBetaToArray(systemIdentState.Beta)[i]);
}
float kpMax = 0.0f;
for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {
double beta = exp(SystemIdentStateBetaToArray(systemIdentState.Beta)[i]);
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d;
double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d;
if (i<2) {
if (kpMax < (float) kp) {
kpMax = (float) kp;
}
} else {
// use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value
// use largest roll/pitch kp because it is the axis most slowed by rotational inertia
// and yaw is also slowed maximally by rotational inertia
// note that kp, ki, kd are all proportional in beta
// so reducing them all proportionally is the same as changing beta
float min = 0.0f;
float max = 0.0f;
switch (systemIdentSettings.YawPIDRatioFunction) {
case SYSTEMIDENTSETTINGS_YAWPIDRATIOFUNCTION_DISABLE:
max = 1000.0f;
min = 0.0f;
break;
case SYSTEMIDENTSETTINGS_YAWPIDRATIOFUNCTION_LIMIT:
max = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMax;
min = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMin;
break;
}
float ratio = 1.0f;
if (min > 0.0f && (float) kp < min) {
ratio = (float) kp / min;
} else if (max > 0.0f && (float) kp > max) {
ratio = (float) kp / max;
}
kp /= (double) ratio;
ki /= (double) ratio;
kd /= (double) ratio;
}
switch(i) {
case 0: // Roll
stabSettingsBank.RollRatePID.Kp = kp;

16
shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -17,18 +17,20 @@
<field name="NoiseMin" units="" type="uint8" elements="1" defaultvalue="8"/>
<field name="NoiseRate" units="" type="uint8" elements="1" defaultvalue="10"/>
<field name="NoiseMax" units="" type="uint8" elements="1" defaultvalue="13"/>
<field name="CalculateYaw" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True"/>
<field name="YawBetaMin" units="" type="float" elements="1" defaultvalue="5.8"/>
<field name="OverrideYawBeta" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True"/>
<field name="CalculateYaw" units="bool" type="enum" elements="1" options="False,True,TrueIgnoreError" defaultvalue="True"/>
<!-- Mateuze quad needs yaw P to be at most 2.6 times roll/pitch P to avoid yaw oscillation -->
<!-- Cliff sluggish 500 quad would like yaw P to be about 5.8 times roll/pitch P, but can easily live with 2.6 -->
<field name="YawToRollPitchPIDRatioMin" units="" type="float" elements="1" defaultvalue="1.0"/>
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.6"/>
<field name="YawPIDRatioFunction" units="" type="enum" elements="1" options="Disable,Limit" defaultvalue="Limit"/>
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/>
<field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/>
<!-- smooth vs. quick PID selector: -->
<!-- 0 = disabled -->
<!-- 10 thru 13 correspond to aux0 -> aux3 transmitter knobs -->
<!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
<!-- 23 and 25 are discrete 3 and 5 stop rount robin selectors accessed by quickly toggling the fms 3 times -->
<!-- where -100 is smoothest, 0 is middle, +100 is quickest -->
<!-- 23 means stops at 0, 100, -100 (repeat) -->
<!-- 25 means stops at 0, 50, 100, -100, -50 (repeat) -->
<!-- 23 (3 stops) means stops at 0%, 100%, -100% (repeat) -->
<!-- 25 (5 stops) means stops at 0%, 50%, 100%, -100%, -50% (repeat) -->
<field name="SmoothQuick" units="" type="uint8" elements="1" defaultvalue="25"/>
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>