public static double[] defaultControllerGains(AsstList type)
{
switch (type)
{
case AsstList.HdgBank:
return(PilotAssistant.defaultHdgBankGains);
case AsstList.BankToYaw:
return(PilotAssistant.defaultBankToYawGains);
case AsstList.Aileron:
return(PilotAssistant.defaultAileronGains);
case AsstList.Rudder:
return(PilotAssistant.defaultRudderGains);
case AsstList.Altitude:
return(PilotAssistant.defaultAltitudeGains);
case AsstList.VertSpeed:
return(PilotAssistant.defaultVSpeedGains);
case AsstList.Elevator:
return(PilotAssistant.defaultElevatorGains);
case AsstList.Speed:
return(PilotAssistant.defaultSpeedGains);
case AsstList.Acceleration:
return(PilotAssistant.defaultAccelGains);
default:
return(PilotAssistant.defaultAileronGains);
}
}
protected double sum = 0; // integral sum
#endregion Fields
#region Constructors
public Asst_PID_Controller(AsstList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double shiftRate = 1)
{
ctrlID = ID;
k_proportional = Kp;
k_integral = Ki;
k_derivative = Kd;
outMin = OutputMin;
outMax = OutputMax;
integralClampLower = intClampLower;
integralClampUpper = intClampUpper;
scale = scalar;
easing = shiftRate;
inMin = -double.MaxValue;
inMax = double.MaxValue;
}
public Asst_PID_Controller(AsstList ID, double[] gains)
{
CtrlID = ID;
K_proportional = gains[0];
K_integral = gains[1];
K_derivative = gains[2];
OutMin = gains[3];
OutMax = gains[4];
IntegralClampLower = gains[5];
IntegralClampUpper = gains[6];
scale = gains[7];
easing = gains[8];
InMin = -double.MaxValue;
InMax = double.MaxValue;
}
public Asst_PID_Controller(AsstList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double shiftRate = 1)
{
CtrlID = ID;
K_proportional = Kp;
K_integral = Ki;
K_derivative = Kd;
OutMin = OutputMin;
OutMax = OutputMax;
IntegralClampLower = intClampLower;
IntegralClampUpper = intClampUpper;
scale = scalar;
easing = shiftRate;
InMin = -double.MaxValue;
InMax = double.MaxValue;
}
public Asst_PID_Controller(AsstList ID, double[] gains)
{
ctrlID = ID;
k_proportional = gains[0];
k_integral = gains[1];
k_derivative = gains[2];
outMin = gains[3];
outMax = gains[4];
integralClampLower = gains[5];
integralClampUpper = gains[6];
scale = gains[7];
easing = gains[8];
inMin = -double.MaxValue;
inMax = double.MaxValue;
}
/// <summary>
/// Processes a config node for a controller into a more accessible array of doubles
/// </summary>
/// <param name="node">A controller node</param>
/// <param name="type">An ID to use for referencing the default values in cases of null input</param>
/// <returns>an array of doubles containing the gains for a controller</returns>
public static double[] GainsArrayFromNode(ConfigNode node, AsstList type)
{
if (ReferenceEquals(node, null))
{
return(DefaultControllerGains(type));
}
double[] gains = new double[9];
double.TryParse(node.GetValue(pGain), out gains[0]);
double.TryParse(node.GetValue(iGain), out gains[1]);
double.TryParse(node.GetValue(dGain), out gains[2]);
double.TryParse(node.GetValue(min), out gains[3]);
double.TryParse(node.GetValue(max), out gains[4]);
double.TryParse(node.GetValue(iLower), out gains[5]);
double.TryParse(node.GetValue(iUpper), out gains[6]);
double.TryParse(node.GetValue(scalar), out gains[7]);
double.TryParse(node.GetValue(ease), out gains[8]);
return(gains);
}
public static double[] controllerGains(ConfigNode node, AsstList type)
{
double[] gains = new double[9];
if (node == null)
{
return(defaultControllerGains(type));
}
double.TryParse(node.GetValue(pGain), out gains[0]);
double.TryParse(node.GetValue(iGain), out gains[1]);
double.TryParse(node.GetValue(dGain), out gains[2]);
double.TryParse(node.GetValue(min), out gains[3]);
double.TryParse(node.GetValue(max), out gains[4]);
double.TryParse(node.GetValue(iLower), out gains[5]);
double.TryParse(node.GetValue(iUpper), out gains[6]);
double.TryParse(node.GetValue(scalar), out gains[7]);
double.TryParse(node.GetValue(ease), out gains[8]);
return(gains);
}
public static double[] controllerGains(ConfigNode node, AsstList type)
{
double[] gains = new double[9];
if (node == null)
return defaultControllerGains(type);
double.TryParse(node.GetValue(pGain), out gains[0]);
double.TryParse(node.GetValue(iGain), out gains[1]);
double.TryParse(node.GetValue(dGain), out gains[2]);
double.TryParse(node.GetValue(min), out gains[3]);
double.TryParse(node.GetValue(max), out gains[4]);
double.TryParse(node.GetValue(iLower), out gains[5]);
double.TryParse(node.GetValue(iUpper), out gains[6]);
double.TryParse(node.GetValue(scalar), out gains[7]);
double.TryParse(node.GetValue(ease), out gains[8]);
return gains;
}
public static double[] defaultControllerGains(AsstList type)
{
switch(type)
{
case AsstList.HdgBank:
return George.defaultHdgBankGains;
case AsstList.BankToYaw:
return George.defaultBankToYawGains;
case AsstList.Aileron:
return George.defaultAileronGains;
case AsstList.Rudder:
return George.defaultRudderGains;
case AsstList.Altitude:
return George.defaultAltitudeGains;
case AsstList.VertSpeed:
return George.defaultVSpeedGains;
case AsstList.Elevator:
return George.defaultElevatorGains;
case AsstList.Speed:
return George.defaultSpeedGains;
case AsstList.Acceleration:
return George.defaultAccelGains;
case AsstList.CdiVelocity:
return George.defaultXtrkGains;
default:
return George.defaultAileronGains;
}
}
public AsstController(AsstList ID, double[] gains)
: base(gains)
{
ctrlID = ID;
}
public AsstController(AsstList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
: base(Kp, Ki, Kd, OutputMin, OutputMax, intClampLower, intClampUpper, scalar, easing)
{
ctrlID = ID;
}
public void SetLimit(double newLimit, AsstList controller)
{
Asst_PID_Controller c = controller.GetAsst(this);
c.outMax = Math.Abs(newLimit);
c.outMin = -Math.Abs(newLimit);
}
public APController GetController(AsstList id)
{
return controllers[(int)id];
}
/// <summary>
/// Processes a config node for a controller into a more accessible array of doubles
/// </summary>
/// <param name="node">A controller node</param>
/// <param name="type">An ID to use for referencing the default values in cases of null input</param>
/// <returns>an array of doubles containing the gains for a controller</returns>
public static double[] gainsArrayFromNode(ConfigNode node, AsstList type)
{
if (ReferenceEquals(node, null))
return defaultControllerGains(type);
double[] gains = new double[9];
double.TryParse(node.GetValue(pGain), out gains[0]);
double.TryParse(node.GetValue(iGain), out gains[1]);
double.TryParse(node.GetValue(dGain), out gains[2]);
double.TryParse(node.GetValue(min), out gains[3]);
double.TryParse(node.GetValue(max), out gains[4]);
double.TryParse(node.GetValue(iLower), out gains[5]);
double.TryParse(node.GetValue(iUpper), out gains[6]);
double.TryParse(node.GetValue(scalar), out gains[7]);
double.TryParse(node.GetValue(ease), out gains[8]);
return gains;
}
public static AsstController GetAsst(this AsstList id, PilotAssistant instance)
{
return(instance.controllers[(int)id]);
}
public AsstController(AsstList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
: base(Kp, Ki, Kd, OutputMin, OutputMax, intClampLower, intClampUpper, scalar, easing)
{
ctrlID = ID;
}
public AsstController(AsstList ID, double[] gains) : base(gains)
{
ctrlID = ID;
}
private void drawPIDvalues(AsstList controllerid, string inputName, string inputUnits, double inputValue, int displayPrecision, string outputName, string outputUnits, bool showTarget = true)
{
Asst_PID_Controller controller = controllerid.GetAsst(this);
controller.bShow = GUILayout.Toggle(controller.bShow, string.Format("{0}: {1}{2}", inputName, inputValue.ToString("N" + displayPrecision.ToString()), inputUnits), GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle], GUILayout.Width(200));
if (controller.bShow)
{
if (showTarget)
{
switch (controllerid)
{
case AsstList.Speed:
case AsstList.Acceleration:
GUILayout.Label("Target: " + (controller.target_setpoint * Utils.speedUnitTransform(units, Vessel.speedOfSound)).ToString("N" + displayPrecision.ToString()) + inputUnits, GUILayout.Width(200));
break;
default:
GUILayout.Label("Target: " + controller.target_setpoint.ToString("N" + displayPrecision.ToString()) + inputUnits, GUILayout.Width(200));
break;
}
}
GUILayout.BeginHorizontal();
GUILayout.BeginVertical();
controller.k_proportional = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.k_proportional.ToString("G3"), 45);
controller.k_integral = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.k_integral.ToString("G3"), 45);
controller.k_derivative = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.k_derivative.ToString("G3"), 45);
controller.Scalar = GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.Scalar.ToString("G3"), 45);
if (showPIDLimits)
{
GUILayout.EndVertical();
GUILayout.BeginVertical();
controller.outMax = GeneralUI.labPlusNumBox(new GUIContent(string.Format("Max {0}{1}:", outputName, outputUnits), OutMaxTooltip), controller.outMax.ToString("G3"));
if (doublesided)
{
controller.outMin = GeneralUI.labPlusNumBox(new GUIContent(string.Format("Min {0}{1}:", outputName, outputUnits), OutMinTooltip), controller.outMin.ToString("G3"));
controller.integralClampLower = GeneralUI.labPlusNumBox(GeneralUI.IMinLabel, controller.integralClampLower.ToString("G3"));
}
else
{
controller.outMin = -controller.outMax;
controller.integralClampLower = -controller.integralClampUpper;
}
controller.integralClampUpper = GeneralUI.labPlusNumBox(GeneralUI.IMaxLabel, controller.integralClampUpper.ToString("G3"));
controller.Easing = GeneralUI.labPlusNumBox(GeneralUI.EasingLabel, controller.Easing.ToString("G3"));
}
GUILayout.EndVertical();
GUILayout.EndHorizontal();
}
}
private void drawPIDvalues(AsstList controllerid, string inputName, string inputUnits, double inputValue, int displayPrecision, string outputName, string outputUnits, bool invertOutput = false, bool showTarget = true)
{
var controller = GetController(controllerid);
controller.bShow = GUILayout.Toggle(controller.bShow, string.Format("{0}: {1}{2}", inputName, inputValue.ToString("N" + displayPrecision.ToString()), inputUnits), GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle], GUILayout.Width(200));
if (controller.bShow)
{
if (showTarget)
GUILayout.Label("Target: " + controller.SetPoint.ToString("N" + displayPrecision.ToString()) + inputUnits, GUILayout.Width(200));
GUILayout.BeginHorizontal();
GUILayout.BeginVertical();
controller.PGain = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.PGain.ToString("G3"), 45);
controller.IGain = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.IGain.ToString("G3"), 45);
controller.DGain = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.DGain.ToString("G3"), 45);
controller.Scalar = GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.Scalar.ToString("G3"), 45);
if (showPIDLimits)
{
GUILayout.EndVertical();
GUILayout.BeginVertical();
if (!invertOutput)
{
controller.OutMax = GeneralUI.labPlusNumBox(new GUIContent(string.Format("Max {0}{1}:", outputName, outputUnits), OutMaxTooltip), controller.OutMax.ToString("G3"));
if (doublesided)
controller.OutMin = GeneralUI.labPlusNumBox(new GUIContent(string.Format("Min {0}{1}:", outputName, outputUnits), OutMinTooltip), controller.OutMin.ToString("G3"));
else
controller.OutMin = -controller.OutMax;
if (doublesided)
controller.ClampLower = GeneralUI.labPlusNumBox(GeneralUI.IMinLabel, controller.ClampLower.ToString("G3"));
else
controller.ClampLower = -controller.ClampUpper;
controller.ClampUpper = GeneralUI.labPlusNumBox(GeneralUI.IMaxLabel, controller.ClampUpper.ToString("G3"));
controller.Easing = GeneralUI.labPlusNumBox(GeneralUI.EasingLabel, controller.Easing.ToString("G3"));
}
else
{ // used when response * -1 is used to get the correct output
controller.OutMin = -1 * GeneralUI.labPlusNumBox(new GUIContent(string.Format("Max {0}{1}:", outputName, outputUnits), OutMaxTooltip), (-controller.OutMin).ToString("G3"));
if (doublesided)
controller.OutMax = -1 * GeneralUI.labPlusNumBox(new GUIContent(string.Format("Min {0}{1}:", outputName, outputUnits), OutMinTooltip), (-controller.OutMax).ToString("G3"));
else
controller.OutMax = -controller.OutMin;
if (doublesided)
controller.ClampUpper = -1 * GeneralUI.labPlusNumBox(GeneralUI.IMinLabel, (-controller.ClampUpper).ToString("G3"));
else
controller.ClampUpper = -controller.ClampLower;
controller.ClampLower = -1 * GeneralUI.labPlusNumBox(GeneralUI.IMaxLabel, (-controller.ClampLower).ToString("G3"));
controller.Easing = GeneralUI.labPlusNumBox(GeneralUI.EasingLabel, controller.Easing.ToString("G3"));
}
}
GUILayout.EndVertical();
GUILayout.EndHorizontal();
}
}
public double GetLimit(AsstList controller)
{
return controller.GetAsst(this).outMax;
}