From 1a909acf1974c5f59ba366451969645d80ea1655 Mon Sep 17 00:00:00 2001 From: T00FatToFly <892388086@qq.com> Date: Mon, 7 Nov 2022 15:05:16 +0800 Subject: [PATCH 1/3] Update IAS definition Was previously EAS --- .../Modules/ProgradeThrustController.cs | 102 ++++++++++++++++-- 1 file changed, 96 insertions(+), 6 deletions(-) diff --git a/AtmosphereAutopilot/Modules/ProgradeThrustController.cs b/AtmosphereAutopilot/Modules/ProgradeThrustController.cs index 947c6ee..d7a5bfc 100644 --- a/AtmosphereAutopilot/Modules/ProgradeThrustController.cs +++ b/AtmosphereAutopilot/Modules/ProgradeThrustController.cs @@ -40,6 +40,96 @@ public struct SpeedSetpoint public const float kts2mps = 0.514444f; public const float mps2kts = 1.0f / kts2mps; + public const float machEpsilon = 0.005f; + + public static float rayleighPitotTubeStagPressureSubSonic(float M, float gamma) + { + float pressure; + + pressure = (M * M) * (gamma - 1) / 2 + 1; + pressure = Mathf.Pow(pressure, gamma / (gamma - 1)); + + return pressure; + } + + public static float rayleighPitotTubeStagPressureSuperSonic(float M, float gamma) + { + float pressure; + + pressure = (gamma + 1) * M; + pressure *= pressure; + pressure /= (4 * gamma * M * M - 2 * (gamma - 1)); + pressure = Mathf.Pow(pressure, gamma / (gamma - 1)); + pressure *= (1 - gamma + 2 * gamma * M * M) / (gamma + 1); + + return pressure; + } + + public static float mps2Ias(float mps, Vessel v) + { + float M = (float)(mps / v.speedOfSound); + float gamma = (float) v.mainBody.atmosphereAdiabaticIndex; + float pressureRatio; + if (M <= 1) + { + pressureRatio = rayleighPitotTubeStagPressureSubSonic(M, gamma); + } + else + { + pressureRatio = rayleighPitotTubeStagPressureSuperSonic(M, gamma); + } + + float velocity = pressureRatio - 1; + velocity *= (float)v.staticPressurekPa * 1000 * 2; + velocity /= 1.225f; + velocity = Mathf.Sqrt(velocity); + return velocity; + } + + public static float rayleighMachFromPitotTubeStagPressure(float pressure, float gamma) + { + float M; + + float soundPres = Mathf.Pow((gamma + 1) / 2, gamma / (gamma - 1)); ; + + if (pressure <= soundPres) + { + M = Mathf.Pow(pressure, (gamma - 1) / gamma); + M = (M - 1) * 2 / (gamma - 1); + M = Mathf.Sqrt(M); + } + else + { + float slope, mnew; + float m1 = 1, m2 = 2; + float pt1 = rayleighPitotTubeStagPressureSuperSonic(m1, gamma) - pressure; + float pt2 = rayleighPitotTubeStagPressureSuperSonic(m2, gamma) - pressure; + do + { + slope = (m2 - m1) / (pt2 - pt1); + mnew = m2 - slope * pt2; + mnew = Mathf.Max(1, mnew); // don't go subsonic + m1 = m2; + m2 = mnew; + pt1 = pt2; + pt2 = rayleighPitotTubeStagPressureSuperSonic(m2, gamma) - pressure; + } while (Mathf.Abs(m2 - m1) > machEpsilon); + M = m2; + } + return M; + } + + public static float ias2Mps(float ias, Vessel v) + { + float gamma = (float)v.mainBody.atmosphereAdiabaticIndex; + + float pressureRatio = ias * ias * 1.225f / (float)v.staticPressurekPa / 1000 / 2 + 1; + + float M = rayleighMachFromPitotTubeStagPressure(pressureRatio, gamma); + + return (float) (M * v.speedOfSound); + } + public SpeedSetpoint(SpeedType type, float value, Vessel v) { this.type = type; @@ -61,9 +151,9 @@ public float convert(SpeedType t) case SpeedType.Knots: return mpersec * mps2kts; case SpeedType.IAS: - return Mathf.Sqrt(mpersec * mpersec * (float)v.atmDensity); + return mps2Ias(mpersec, v); case SpeedType.KIAS: - return Mathf.Sqrt(mpersec * mpersec * mps2kts * mps2kts * (float)v.atmDensity); + return mps2Ias(mpersec, v) * mps2kts; default: return 0.0f; } @@ -80,9 +170,9 @@ public static float convert_from_mps(SpeedType t, float mps, Vessel v) case SpeedType.Knots: return mps * mps2kts; case SpeedType.IAS: - return Mathf.Sqrt(mps * mps * (float)v.atmDensity); + return mps2Ias(mps, v); case SpeedType.KIAS: - return Mathf.Sqrt(mps * mps * mps2kts * mps2kts * (float)v.atmDensity); + return mps2Ias(mps, v) * mps2kts; default: return 0.0f; } @@ -99,9 +189,9 @@ public float mps() case SpeedType.Knots: return value * kts2mps; case SpeedType.IAS: - return Mathf.Sqrt(value * value / (float)v.atmDensity); + return ias2Mps(value, v); case SpeedType.KIAS: - return Mathf.Sqrt(value * value * kts2mps * kts2mps / (float)v.atmDensity); + return ias2Mps(value * kts2mps, v); default: return 0.0f; } From c5ed4b0225ff7b0f0852e977f804fbb697b21769 Mon Sep 17 00:00:00 2001 From: T00FatToFly <892388086@qq.com> Date: Wed, 16 Nov 2022 16:04:38 +0800 Subject: [PATCH 2/3] Revert "Update IAS definition" This reverts commit 1a909acf1974c5f59ba366451969645d80ea1655. --- .../Modules/ProgradeThrustController.cs | 102 ++---------------- 1 file changed, 6 insertions(+), 96 deletions(-) diff --git a/AtmosphereAutopilot/Modules/ProgradeThrustController.cs b/AtmosphereAutopilot/Modules/ProgradeThrustController.cs index d7a5bfc..947c6ee 100644 --- a/AtmosphereAutopilot/Modules/ProgradeThrustController.cs +++ b/AtmosphereAutopilot/Modules/ProgradeThrustController.cs @@ -40,96 +40,6 @@ public struct SpeedSetpoint public const float kts2mps = 0.514444f; public const float mps2kts = 1.0f / kts2mps; - public const float machEpsilon = 0.005f; - - public static float rayleighPitotTubeStagPressureSubSonic(float M, float gamma) - { - float pressure; - - pressure = (M * M) * (gamma - 1) / 2 + 1; - pressure = Mathf.Pow(pressure, gamma / (gamma - 1)); - - return pressure; - } - - public static float rayleighPitotTubeStagPressureSuperSonic(float M, float gamma) - { - float pressure; - - pressure = (gamma + 1) * M; - pressure *= pressure; - pressure /= (4 * gamma * M * M - 2 * (gamma - 1)); - pressure = Mathf.Pow(pressure, gamma / (gamma - 1)); - pressure *= (1 - gamma + 2 * gamma * M * M) / (gamma + 1); - - return pressure; - } - - public static float mps2Ias(float mps, Vessel v) - { - float M = (float)(mps / v.speedOfSound); - float gamma = (float) v.mainBody.atmosphereAdiabaticIndex; - float pressureRatio; - if (M <= 1) - { - pressureRatio = rayleighPitotTubeStagPressureSubSonic(M, gamma); - } - else - { - pressureRatio = rayleighPitotTubeStagPressureSuperSonic(M, gamma); - } - - float velocity = pressureRatio - 1; - velocity *= (float)v.staticPressurekPa * 1000 * 2; - velocity /= 1.225f; - velocity = Mathf.Sqrt(velocity); - return velocity; - } - - public static float rayleighMachFromPitotTubeStagPressure(float pressure, float gamma) - { - float M; - - float soundPres = Mathf.Pow((gamma + 1) / 2, gamma / (gamma - 1)); ; - - if (pressure <= soundPres) - { - M = Mathf.Pow(pressure, (gamma - 1) / gamma); - M = (M - 1) * 2 / (gamma - 1); - M = Mathf.Sqrt(M); - } - else - { - float slope, mnew; - float m1 = 1, m2 = 2; - float pt1 = rayleighPitotTubeStagPressureSuperSonic(m1, gamma) - pressure; - float pt2 = rayleighPitotTubeStagPressureSuperSonic(m2, gamma) - pressure; - do - { - slope = (m2 - m1) / (pt2 - pt1); - mnew = m2 - slope * pt2; - mnew = Mathf.Max(1, mnew); // don't go subsonic - m1 = m2; - m2 = mnew; - pt1 = pt2; - pt2 = rayleighPitotTubeStagPressureSuperSonic(m2, gamma) - pressure; - } while (Mathf.Abs(m2 - m1) > machEpsilon); - M = m2; - } - return M; - } - - public static float ias2Mps(float ias, Vessel v) - { - float gamma = (float)v.mainBody.atmosphereAdiabaticIndex; - - float pressureRatio = ias * ias * 1.225f / (float)v.staticPressurekPa / 1000 / 2 + 1; - - float M = rayleighMachFromPitotTubeStagPressure(pressureRatio, gamma); - - return (float) (M * v.speedOfSound); - } - public SpeedSetpoint(SpeedType type, float value, Vessel v) { this.type = type; @@ -151,9 +61,9 @@ public float convert(SpeedType t) case SpeedType.Knots: return mpersec * mps2kts; case SpeedType.IAS: - return mps2Ias(mpersec, v); + return Mathf.Sqrt(mpersec * mpersec * (float)v.atmDensity); case SpeedType.KIAS: - return mps2Ias(mpersec, v) * mps2kts; + return Mathf.Sqrt(mpersec * mpersec * mps2kts * mps2kts * (float)v.atmDensity); default: return 0.0f; } @@ -170,9 +80,9 @@ public static float convert_from_mps(SpeedType t, float mps, Vessel v) case SpeedType.Knots: return mps * mps2kts; case SpeedType.IAS: - return mps2Ias(mps, v); + return Mathf.Sqrt(mps * mps * (float)v.atmDensity); case SpeedType.KIAS: - return mps2Ias(mps, v) * mps2kts; + return Mathf.Sqrt(mps * mps * mps2kts * mps2kts * (float)v.atmDensity); default: return 0.0f; } @@ -189,9 +99,9 @@ public float mps() case SpeedType.Knots: return value * kts2mps; case SpeedType.IAS: - return ias2Mps(value, v); + return Mathf.Sqrt(value * value / (float)v.atmDensity); case SpeedType.KIAS: - return ias2Mps(value * kts2mps, v); + return Mathf.Sqrt(value * value * kts2mps * kts2mps / (float)v.atmDensity); default: return 0.0f; } From d8395c119547d34bfdac4cf24f81f7a2078d8ba8 Mon Sep 17 00:00:00 2001 From: T00FatToFly <892388086@qq.com> Date: Wed, 16 Nov 2022 22:04:12 +0800 Subject: [PATCH 3/3] Update IAS definition v2 This version of IAS def uses reflection to read IAS value from FAR API, and vanilla Vessel.indicatedAirSpeed() method for stock aerodynamic. Secant method is still required for IAS->mps conversions but only invoked when explicitly requested (and NOT per-frame). Prograde Thrust Controller refactored to allow two airspeed reference systems: kinematic-based (surface speed) and pressure-based (indicated airspeed). Also added option to turn off Speed Control if throttle full / cut out key is pressed. --- AtmosphereAutopilot/AtmosphereAutopilot.cs | 19 +- .../AtmosphereAutopilot.csproj | 2 + AtmosphereAutopilot/Common.cs | 83 ++++ AtmosphereAutopilot/GUI/NeoGUIController.cs | 4 +- .../Modules/AoAHoldController.cs | 2 +- .../Modules/CruiseController.cs | 9 +- AtmosphereAutopilot/Modules/MouseDirector.cs | 2 +- .../Modules/ProgradeThrustController.cs | 375 ++++++++++++++---- .../Modules/StandardFlyByWire.cs | 2 +- .../Modules/StateController.cs | 17 + .../Reflections/FARReflections.cs | 96 +++++ .../Reflections/ReflectionUtils.cs | 58 +++ 12 files changed, 571 insertions(+), 98 deletions(-) create mode 100644 AtmosphereAutopilot/Reflections/FARReflections.cs create mode 100644 AtmosphereAutopilot/Reflections/ReflectionUtils.cs diff --git a/AtmosphereAutopilot/AtmosphereAutopilot.cs b/AtmosphereAutopilot/AtmosphereAutopilot.cs index e54bd1b..dabe5de 100644 --- a/AtmosphereAutopilot/AtmosphereAutopilot.cs +++ b/AtmosphereAutopilot/AtmosphereAutopilot.cs @@ -84,20 +84,15 @@ public enum AerodinamycsModel /// Current aerodynamics model. /// public static AerodinamycsModel AeroModel { get; private set; } - Assembly far_assembly; + public FARReflections farReflections; void determine_aerodynamics() { AeroModel = AerodinamycsModel.Stock; - foreach (var a in AppDomain.CurrentDomain.GetAssemblies()) + farReflections = new FARReflections(); + if (farReflections.isFarFound) { - if (a.GetName().Name.Equals("FerramAerospaceResearch")) - { - far_assembly = a; - AeroModel = AerodinamycsModel.FAR; - Debug.Log("[AtmosphereAutopilot]: FAR aerodynamics detected"); - return; - } + AeroModel = AerodinamycsModel.FAR; } } @@ -111,11 +106,7 @@ void get_csurf_module() if (AeroModel == AerodinamycsModel.Stock) control_surface_module_type = typeof(SyncModuleControlSurface); else - { - control_surface_module_type = far_assembly.GetTypes().First(t => t.Name.Equals("FARControllableSurface")); - if (control_surface_module_type == null) - throw new Exception("AtmosphereAutopilot could not bind to FAR FARControllableSurface class"); - } + control_surface_module_type = farReflections.FARControllableSurfaceType; } public static Dictionary gimbal_module_wrapper_map = new Dictionary(4); diff --git a/AtmosphereAutopilot/AtmosphereAutopilot.csproj b/AtmosphereAutopilot/AtmosphereAutopilot.csproj index e8748ce..eec7899 100644 --- a/AtmosphereAutopilot/AtmosphereAutopilot.csproj +++ b/AtmosphereAutopilot/AtmosphereAutopilot.csproj @@ -102,6 +102,7 @@ + @@ -157,6 +158,7 @@ + diff --git a/AtmosphereAutopilot/Common.cs b/AtmosphereAutopilot/Common.cs index 7887a50..1838625 100644 --- a/AtmosphereAutopilot/Common.cs +++ b/AtmosphereAutopilot/Common.cs @@ -199,6 +199,89 @@ public static string ToString(this Vector3d v, string format) { return '(' + v.x.ToString(format) + ", " + v.y.ToString(format) + ", " + v.z.ToString(format) + ')'; } + + public enum AlgoStatus + { + Success, + InvalidArgument, + ConvergenceFailure, + MaxIterReached, + NaN, + OutOfBounds, + } + + /// + /// Try to find a root for a target result of a function using secant method. + /// + /// Function to find root of + /// Target result of the function + /// Initial first guess + /// Initial second guess + /// Convergence tolerance + /// Maximum iterations allowed + /// Minimum value of the root + /// Maximum value of the root + /// A tuple of algorithm status and a possible root + public static Tuple Secant(Func function, + double target, + double x0, + double x1, + double epsilon, + int maxIter, + double min = double.NegativeInfinity, + double max = double.PositiveInfinity) + { + if (x0 == x1) + return new Tuple(AlgoStatus.InvalidArgument, x0); + + double slope, xnew; + int boundFlag = 0; + double v0 = function(x0) - target; + double v1 = function(x1) - target; + + if (double.IsNaN(v0) || double.IsNaN(v1)) + return new Tuple(AlgoStatus.NaN, x0); + + for (int i = 0; i < maxIter; i++) + { + if (Math.Abs(x1 - x0) <= epsilon) + return new Tuple(AlgoStatus.Success, x1); + + slope = (x1 - x0) / (v1 - v0); + if (double.IsNaN(slope) || double.IsInfinity(slope)) + return new Tuple(AlgoStatus.ConvergenceFailure, x1); + xnew = x1 - slope * v1; + if (xnew < min) + { + if (boundFlag == -1) + return new Tuple(AlgoStatus.OutOfBounds, min); + else + { + xnew = min; + boundFlag = -1; + } + } + if (xnew > max) + { + if (boundFlag == 1) + return new Tuple(AlgoStatus.OutOfBounds, max); + else + { + xnew = max; + boundFlag = 1; + } + } + x0 = x1; + x1 = xnew; + v0 = v1; + v1 = function(x1) - target; + if (double.IsNaN(v1)) + return new Tuple(AlgoStatus.NaN, x1); + } + if (Math.Abs(x0 - x1) < epsilon) + return new Tuple(AlgoStatus.Success, x1); + return new Tuple(AlgoStatus.MaxIterReached, x1); + } } public static class VesselExtensions diff --git a/AtmosphereAutopilot/GUI/NeoGUIController.cs b/AtmosphereAutopilot/GUI/NeoGUIController.cs index ca5a612..6c77ec4 100644 --- a/AtmosphereAutopilot/GUI/NeoGUIController.cs +++ b/AtmosphereAutopilot/GUI/NeoGUIController.cs @@ -132,13 +132,13 @@ public float speed { get { if (speedAP == null) return 0f; - return speedAP.setpoint.mps(); + return speedAP.setpoint.sameSystemMps(); } set { if (parent.ActiveVessel != null) { speedAP.setpoint_field.Value = value; - speedAP.chosen_spd_mode = 1; + speedAP.type = SpeedType.MetersPerSecond; speedAP.setpoint = new SpeedSetpoint(SpeedType.MetersPerSecond, value, parent.ActiveVessel); } } diff --git a/AtmosphereAutopilot/Modules/AoAHoldController.cs b/AtmosphereAutopilot/Modules/AoAHoldController.cs index efdf738..423346d 100644 --- a/AtmosphereAutopilot/Modules/AoAHoldController.cs +++ b/AtmosphereAutopilot/Modules/AoAHoldController.cs @@ -115,7 +115,7 @@ public override void ApplyControl(FlightCtrlState cntrl) return; if (thrust_c.spd_control_enabled) - thrust_c.ApplyControl(cntrl, thrust_c.setpoint.mps()); + thrust_c.ApplyControl(cntrl); if (use_keys) ControlUtils.neutralize_user_input(cntrl, PITCH); diff --git a/AtmosphereAutopilot/Modules/CruiseController.cs b/AtmosphereAutopilot/Modules/CruiseController.cs index 4d992f0..f3a5630 100644 --- a/AtmosphereAutopilot/Modules/CruiseController.cs +++ b/AtmosphereAutopilot/Modules/CruiseController.cs @@ -87,7 +87,7 @@ public override void ApplyControl(FlightCtrlState cntrl) return; if (thrust_c.spd_control_enabled) - thrust_c.ApplyControl(cntrl, thrust_c.setpoint.mps()); + thrust_c.ApplyControl(cntrl); desired_velocity = Vector3d.zero; planet2ves = vessel.ReferenceTransform.position - vessel.mainBody.position; @@ -320,14 +320,15 @@ Vector3d account_for_height(Vector3d desired_direction) else effective_max_climb_angle = 1.0; - double spd_diff = (imodel.surface_v_magnitude - thrust_c.setpoint.mps()); + double spd_diff_magnitude; + double spd_diff = thrust_c.speedDiff(out spd_diff_magnitude); if (spd_diff < -flc_margin) effective_max_climb_angle *= 0.0; else if (spd_diff < 0.0) - effective_max_climb_angle *= (spd_diff + flc_margin) / flc_margin; + effective_max_climb_angle *= (spd_diff * spd_diff_magnitude + flc_margin) / flc_margin; } else - effective_max_climb_angle *= Math.Max(0.0, Math.Min(1.0, vessel.srfSpeed / thrust_c.setpoint.mps())); + effective_max_climb_angle *= Common.Clamp(thrust_c.currentSpeedOfSameSystem() / thrust_c.setpoint.sameSystemMps(), 0.0, 1.0); } double max_vert_speed = vessel.horizontalSrfSpeed * Math.Tan(effective_max_climb_angle * dgr2rad); diff --git a/AtmosphereAutopilot/Modules/MouseDirector.cs b/AtmosphereAutopilot/Modules/MouseDirector.cs index d09be7c..c0c1444 100644 --- a/AtmosphereAutopilot/Modules/MouseDirector.cs +++ b/AtmosphereAutopilot/Modules/MouseDirector.cs @@ -66,7 +66,7 @@ public override void ApplyControl(FlightCtrlState cntrl) dir_c.ApplyControl(cntrl, camera_direction, Vector3d.zero); if (thrust_c.spd_control_enabled) - thrust_c.ApplyControl(cntrl, thrust_c.setpoint.mps()); + thrust_c.ApplyControl(cntrl); } //bool camera_correct = false; diff --git a/AtmosphereAutopilot/Modules/ProgradeThrustController.cs b/AtmosphereAutopilot/Modules/ProgradeThrustController.cs index 947c6ee..77ac248 100644 --- a/AtmosphereAutopilot/Modules/ProgradeThrustController.cs +++ b/AtmosphereAutopilot/Modules/ProgradeThrustController.cs @@ -31,6 +31,36 @@ public enum SpeedType KIAS } + public static class SpeedTypeMethods + { + // SpeedType classification + public static bool isKinematicSystem(this SpeedType t) + { + switch (t) + { + case SpeedType.MetersPerSecond: + case SpeedType.Knots: + case SpeedType.Mach: + return true; + case SpeedType.IAS: + case SpeedType.KIAS: + return false; + default: + throw new NotImplementedException(); + } + } + + public static bool isPressureSystem(this SpeedType t) + { + return !isKinematicSystem(t); + } + + public static bool isSameSystem(this SpeedType t1, SpeedType t2) + { + return isKinematicSystem(t1) == isKinematicSystem(t2); + } + } + public struct SpeedSetpoint { public SpeedType type; @@ -47,77 +77,171 @@ public SpeedSetpoint(SpeedType type, float value, Vessel v) this.v = v; } - public float convert(SpeedType t) + /// + /// Convert to target SpeedType. + /// WARNING: protentially time-consuming; accurate result NOT guaranteed + /// + /// The target SpeedType + /// Scalar value of the target speed type + public SpeedSetpoint convert(SpeedType t) { if (t == type) - return value; - float mpersec = mps(); - switch (t) + return this; + + float mps = sameSystemMps(); + if (type.isSameSystem(t)) + return fromSameSystemMps(t, mps, v); + else + { + // converting between systems + if (type.isKinematicSystem()) + return fromSameSystemMps(t, mps2EIAS(mps), v); + else + return fromSameSystemMps(t, eias2Mps(mps), v); + } + } + + + // unit convertions within the same system. + + /// + /// Get speed in m/s of the current airspeed reference system. + /// If converting between systems, use convert() instead. + /// + /// Speed in m/s + public float sameSystemMps() + { + switch (type) { case SpeedType.MetersPerSecond: - return mpersec; - case SpeedType.Mach: - return (float)(mpersec / v.speedOfSound); - case SpeedType.Knots: - return mpersec * mps2kts; case SpeedType.IAS: - return Mathf.Sqrt(mpersec * mpersec * (float)v.atmDensity); + return value; + case SpeedType.Knots: case SpeedType.KIAS: - return Mathf.Sqrt(mpersec * mpersec * mps2kts * mps2kts * (float)v.atmDensity); + return value * kts2mps; + case SpeedType.Mach: + return value * (float)v.speedOfSound; default: - return 0.0f; + throw new NotImplementedException(); } } - public static float convert_from_mps(SpeedType t, float mps, Vessel v) + private static SpeedSetpoint fromSameSystemMps(SpeedType t, float mps, Vessel v) { switch (t) { case SpeedType.MetersPerSecond: - return mps; - case SpeedType.Mach: - return (float)(mps / v.speedOfSound); - case SpeedType.Knots: - return mps * mps2kts; case SpeedType.IAS: - return Mathf.Sqrt(mps * mps * (float)v.atmDensity); + return new SpeedSetpoint(t, mps, v); + case SpeedType.Knots: case SpeedType.KIAS: - return Mathf.Sqrt(mps * mps * mps2kts * mps2kts * (float)v.atmDensity); + return new SpeedSetpoint(t, mps * mps2kts, v); + case SpeedType.Mach: + float m = mps / (float)v.speedOfSound; + if (float.IsNaN(m) || float.IsInfinity(m)) + return new SpeedSetpoint(t, 0.0f, v); + else + return new SpeedSetpoint(t, m, v); default: - return 0.0f; + throw new NotImplementedException(); } } - public float mps() + // converting between the kinematic system and the pressure system + // mps <=> IAS if FAR methods are available + // mps <=> EAS if otherwise + private float mps2EIAS(float mps) { - if (type == SpeedType.MetersPerSecond) - return value; - switch (type) + var RayleighPitotTubeStagPressureDelegate = AtmosphereAutopilot.Instance.farReflections.RayleighPitotTubeStagPressureDelegate; + if (RayleighPitotTubeStagPressureDelegate != null) + return mps2IAS(mps, RayleighPitotTubeStagPressureDelegate.Invoke); + else + return mps2EAS(mps); + } + + private float eias2Mps(float eias) + { + var RayleighPitotTubeStagPressureDelegate = AtmosphereAutopilot.Instance.farReflections.RayleighPitotTubeStagPressureDelegate; + if (RayleighPitotTubeStagPressureDelegate != null) + return ias2Mps(eias, RayleighPitotTubeStagPressureDelegate.Invoke); + else + return eas2Mps(eias); + } + + // IAS convertions if FAR methods are available + private float mps2IAS(float mps, Func rayleighPitotTubeStagPressure) + { + double M = mps / v.speedOfSound; + + if (double.IsNaN(M) || double.IsInfinity(M)) + return 0.0f; + + double presRatio = rayleighPitotTubeStagPressure(M); + + double velocity = presRatio - 1; + velocity *= v.staticPressurekPa * 1000 * 2; + velocity /= 1.225; + velocity = Math.Sqrt(velocity); + + return (float)velocity; + } + private float ias2Mps(float ias, Func rayleighPitotTubeStagPressure) + { + double presRatio = ias * ias * 1.225 / v.staticPressurekPa / 1000 / 2 + 1; + + if (double.IsNaN(presRatio) || presRatio <= 0.0 || presRatio >= 1e6) + return 0.0f; + + var result = Common.Secant( + rayleighPitotTubeStagPressure, + presRatio, + 0.0, + 2.0, + 1e-7, + 50, + 0.0 + ); + + if (result.Item1 != Common.AlgoStatus.Success) { - case SpeedType.Mach: - return (float)(value * v.speedOfSound); - case SpeedType.Knots: - return value * kts2mps; - case SpeedType.IAS: - return Mathf.Sqrt(value * value / (float)v.atmDensity); - case SpeedType.KIAS: - return Mathf.Sqrt(value * value * kts2mps * kts2mps / (float)v.atmDensity); - default: - return 0.0f; + var debugOutput = new { ias, presRatio, v.speedOfSound, v.mainBody.name, v.mainBody.atmosphereAdiabaticIndex }; + + Debug.LogWarning("[AtmosphereAutopilot]: Secant method failed (" + result.Item1.ToString() + ") for ias2Mps; debug info " + debugOutput.ToString()); } + double M = result.Item2; + + return (float)(M * v.speedOfSound); + } + + // Fall-back EAS convertions + private float mps2EAS(float mps) + { + return Mathf.Sqrt(mps * mps * (float)v.atmDensity); + } + private float eas2Mps(float eas) + { + float mps = Mathf.Sqrt(eas * eas / (float)v.atmDensity); + if (float.IsNaN(mps) || float.IsInfinity(mps)) + return 0.0f; + return mps; } } /// /// Naive thrust controller for regular plane flight. /// - public sealed class ProgradeThrustController : SISOController + public sealed class ProgradeThrustController : SubStateController { FlightModel imodel; + FARReflections farReflections; + internal ProgradeThrustController(Vessel v) : base(v, "Prograde thrust controller", 88437224) { + // FAR IAS support + farReflections = AtmosphereAutopilot.Instance.farReflections; + pid.IntegralClamp = double.PositiveInfinity; pid.AccumulDerivClamp = double.PositiveInfinity; pid.KP = 0.4; @@ -176,15 +300,101 @@ protected override void OnDeactivate() private bool was_breaking_previously = true; + /// + /// Get current surface speed in m/s + /// + /// Surface speed in m/s + public double currentSurfaceSpeed() + { + return imodel.surface_v_magnitude; + } + + /// + /// Get current IAS speed in m/s + /// + /// IAS in m/s + public double currentIAS() + { + var activeVesselIASDelegate = farReflections.ActiveVesselIASDelegate; + if (activeVesselIASDelegate != null) + return activeVesselIASDelegate(); + else + return vessel.indicatedAirSpeed; + } + + /// + /// Get current speed in m/s that is the same to the setpoint airspeed reference system + /// + /// Speed in m/s + public double currentSpeedOfSameSystem() + { + if (setpoint.type.isKinematicSystem()) + return currentSurfaceSpeed(); + else + return currentIAS(); + } + + /// + /// Get current speed in m/s that is the same to the setpoint airspeed reference system + /// + /// Ratio of kinematic speed to current speed system + /// Speed in m/s + public double currentSpeedOfSameSystem(out double magnitude) + { + if (setpoint.type.isKinematicSystem()) + { + magnitude = 1.0; + return currentSurfaceSpeed(); + } + else + { + double ss = currentSurfaceSpeed(); + double ias = currentIAS(); + magnitude = ss / ias; + return ias; + } + } + + /// + /// Get speed difference in m/s to the setpoint airspeed + /// + /// Speed difference in m/s + public double speedDiff() + { + return currentSpeedOfSameSystem() - setpoint.sameSystemMps(); + } + + /// + /// Get speed difference in m/s to the setpoint airspeed + /// + /// Ratio of kinematic speed to current speed system + /// Speed difference in m/s + public double speedDiff(out double magnitude) + { + if (setpoint.type.isKinematicSystem()) + { + magnitude = 1.0; + return currentSurfaceSpeed() - setpoint.sameSystemMps(); + } + else + { + double ss = currentSurfaceSpeed(); + double ias = currentIAS(); + magnitude = ss / ias; + return ias - setpoint.sameSystemMps(); + } + } + /// /// Main control function /// /// Control state to change - /// Prograde surface speed setpoint - public override float ApplyControl(FlightCtrlState cntrl, float target_value) + public override void ApplyControl(FlightCtrlState cntrl) { - current_v = imodel.surface_v_magnitude; - desired_v = target_value; + double presAccMagnitude; + current_v = currentSpeedOfSameSystem(out presAccMagnitude); + + desired_v = setpoint.sameSystemMps(); // apply breaks if needed if (use_breaks) @@ -243,7 +453,7 @@ public override float ApplyControl(FlightCtrlState cntrl, float target_value) drag_estimate = current_acc - prograde_thrust / imodel.sum_mass - Vector3d.Dot(imodel.gravity_acc + imodel.noninert_acc, surfspd_dir); v_error = desired_v - current_v; - desired_acc = Kp_v * v_error; + desired_acc = Kp_v * v_error * presAccMagnitude; if (Math.Abs(desired_acc - current_acc) < relaxation_acc_error) { // we're on low error regime, let's smooth out acceleration error using exponential moving average @@ -262,7 +472,22 @@ public override float ApplyControl(FlightCtrlState cntrl, float target_value) } prev_input = cntrl.mainThrottle; - return cntrl.mainThrottle; + } + + /// + /// Supplemental control function allows direct control over the throttle + /// + /// Control state to change + /// Set throttle value (0.0f ~ 1.0f) + public void ApplyThrottle(FlightCtrlState cntrl, float throttle) + { + if (use_breaks && was_breaking_previously) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false); + was_breaking_previously = false; + } + + cntrl.mainThrottle = Mathf.Clamp01(throttle); } //double prev_thrust; @@ -403,13 +628,16 @@ float solve_thrust_req(double required_thrust, float prev_throttle) #region GUI - static readonly string[] spd_str_arr = new string[] { "OFF", "ms", "kts", "M", "ias", "kias" }; + string[] spd_str_arr = new string[] { "m/s", "kts", "Mach", "IAS", "kIAS" }; + string SpdStr(SpeedType t) + { + return spd_str_arr[(int)t]; + } - public int chosen_spd_mode = 0; public bool spd_control_enabled = false; [VesselSerializable("spd_type")] - SpeedType type = SpeedType.MetersPerSecond; + public SpeedType type = SpeedType.MetersPerSecond; [VesselSerializable("setpoint_field")] internal DelayedFieldFloat setpoint_field = new DelayedFieldFloat(100.0f, "G4"); @@ -430,6 +658,10 @@ float solve_thrust_req(double required_thrust, float prev_throttle) [GlobalSerializable("use_throttle_hotkeys")] public static bool use_throttle_hotkeys = true; + [AutoGuiAttr("disable_on_thr_full_cut", true)] + [GlobalSerializable("disable_on_thr_full_cut")] + public static bool disable_on_thr_full_cut = false; + [GlobalSerializable("spd_control_toggle_key")] [AutoHotkeyAttr("Speed control toggle")] static KeyCode spd_control_toggle_key = KeyCode.None; @@ -444,6 +676,18 @@ public override void OnUpdate() //changed = true; } + if (disable_on_thr_full_cut && spd_control_enabled && + !FlightDriver.Pause && InputLockManager.IsUnlocked(ControlTypes.THROTTLE)) + { + if (GameSettings.THROTTLE_FULL.GetKey() || + GameSettings.THROTTLE_CUTOFF.GetKey()) + { + // turn speed control off if throttle full/cutoff key is pressed + spd_control_enabled = false; + MessageManager.post_status_message("Speed control disabled"); + } + } + if (use_throttle_hotkeys && spd_control_enabled && !FlightDriver.Pause && InputLockManager.IsUnlocked(ControlTypes.THROTTLE)) { @@ -494,25 +738,7 @@ protected override void OnGUICustomAlways() if (need_to_show_change) { Rect rect = new Rect(Screen.width / 2.0f - 80.0f, 120.0f, 160.0f, 20.0f); - string str = "SPD = " + setpoint.value.ToString("G4"); - switch (setpoint.type) - { - case SpeedType.IAS: - str = str + " IAS"; - break; - case SpeedType.KIAS: - str = str + " kIAS"; - break; - case SpeedType.Knots: - str = str + " kts"; - break; - case SpeedType.Mach: - str = str + " M"; - break; - case SpeedType.MetersPerSecond: - str = str + " m/s"; - break; - } + string str = "SPD = " + setpoint.value.ToString("G4") + " " + SpdStr(setpoint.type); GUI.Label(rect, str, GUIStyles.hoverLabel); } @@ -527,25 +753,24 @@ public bool SpeedCtrlGUIBlock() { spd_control_enabled = GUILayout.Toggle(spd_control_enabled, "Speed control", GUIStyles.toggleButtonStyle); GUILayout.BeginHorizontal(); - for (int i = 1; i < 6; i++) + + SpeedType oldType = type; + for (int i = 0; i < 5; i++) { - if (GUILayout.Toggle(chosen_spd_mode == i, spd_str_arr[i], GUIStyles.toggleButtonStyle)) - chosen_spd_mode = i; + if (GUILayout.Toggle(type == (SpeedType)i, spd_str_arr[i], GUIStyles.toggleButtonStyle)) + { + type = (SpeedType)i; + } } GUILayout.EndHorizontal(); - SpeedType newtype = type; - if (chosen_spd_mode != 0) - newtype = (SpeedType)(chosen_spd_mode - 1); - setpoint_field.DisplayLayout(GUIStyles.textBoxStyle); - if (newtype != type) + if (oldType != type) { // need to convert old setpoint to new format - setpoint_field.Value = setpoint.convert(newtype); - setpoint = new SpeedSetpoint(newtype, setpoint_field, vessel); - type = newtype; + setpoint = setpoint.convert(type); + setpoint_field.Value = setpoint.value; } else setpoint = new SpeedSetpoint(type, setpoint_field, vessel); diff --git a/AtmosphereAutopilot/Modules/StandardFlyByWire.cs b/AtmosphereAutopilot/Modules/StandardFlyByWire.cs index 1bdce40..eb9cd4b 100644 --- a/AtmosphereAutopilot/Modules/StandardFlyByWire.cs +++ b/AtmosphereAutopilot/Modules/StandardFlyByWire.cs @@ -202,7 +202,7 @@ public override void ApplyControl(FlightCtrlState cntrl) } if (tc.spd_control_enabled) - tc.ApplyControl(cntrl, tc.setpoint.mps()); + tc.ApplyControl(cntrl); pc.user_controlled = true; if (coord_turn) diff --git a/AtmosphereAutopilot/Modules/StateController.cs b/AtmosphereAutopilot/Modules/StateController.cs index e18b0b5..f01c529 100644 --- a/AtmosphereAutopilot/Modules/StateController.cs +++ b/AtmosphereAutopilot/Modules/StateController.cs @@ -39,6 +39,23 @@ protected StateController(Vessel cur_vessel, string module_name, int wnd_id) } + /// + /// Base class of sub state controllers that is called by main state controllers + /// + public abstract class SubStateController : AutopilotModule + { + protected SubStateController(Vessel cur_vessel, string module_name, int wnd_id) + : base(cur_vessel, wnd_id, module_name) + { } + + /// + /// Main control function of the sub autopilot. + /// + /// Control state to change + public abstract void ApplyControl(FlightCtrlState cntrl); + + } + /// /// Flight control state controller with SIMO base class /// diff --git a/AtmosphereAutopilot/Reflections/FARReflections.cs b/AtmosphereAutopilot/Reflections/FARReflections.cs new file mode 100644 index 0000000..d962d13 --- /dev/null +++ b/AtmosphereAutopilot/Reflections/FARReflections.cs @@ -0,0 +1,96 @@ +/* +Atmosphere Autopilot, plugin for Kerbal Space Program. +Copyright (C) 2015-2016, Baranin Alexander aka Boris-Barboris. + +Atmosphere Autopilot is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. +Atmosphere Autopilot is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. +You should have received a copy of the GNU General Public License +along with Atmosphere Autopilot. If not, see . +*/ + + +using System; +using System.Reflection; +using UnityEngine; + +namespace AtmosphereAutopilot +{ + public class FARReflections + { + public Assembly farAssembly; + public bool isFarFound = false; + + public Type FARControllableSurfaceType; + + public delegate double ActiveVesselIASDelegateType(); + public ActiveVesselIASDelegateType ActiveVesselIASDelegate; + + public delegate double RayleighPitotTubeStagPressureDelegateType(double M); + public RayleighPitotTubeStagPressureDelegateType RayleighPitotTubeStagPressureDelegate; + + public FARReflections() + { + farAssembly = ReflectionUtils.GetAssembly("FerramAerospaceResearch"); + + if (farAssembly != null) + { + Debug.Log("[AtmosphereAutopilot]: FAR aerodynamics detected"); + isFarFound = true; + + // Get FAR control surface type + FARControllableSurfaceType = ReflectionUtils.GetType(farAssembly, "FARControllableSurface"); + if (FARControllableSurfaceType == null) + { + throw new Exception("AtmosphereAutopilot could not bind to FAR FARControllableSurface class"); + } + + // Load ActiveVesselIAS method from FARAPI + var activeVesselIASMethodInfo = ReflectionUtils.GetMethodInfo( + farAssembly, + "FerramAerospaceResearch.FARAPI", + "ActiveVesselIAS", + BindingFlags.Public | BindingFlags.Static); + + if (activeVesselIASMethodInfo == null) + { + Debug.LogWarning("[AtmosphereAutopilot]: FAR ActiveVesselIAS() method reflection failed, disabling FAR IAS support"); + // don't load other methods if ActiveVesselIAS fails + return; + } + else + { + ActiveVesselIASDelegate = (ActiveVesselIASDelegateType) + Delegate.CreateDelegate( + typeof(ActiveVesselIASDelegateType), + activeVesselIASMethodInfo); + } + + // Load undocumented RayleighPitotTubeStagPressure method from FAR + var rayleighPitotTubeStagPressureMethodInfo = ReflectionUtils.GetMethodInfo( + farAssembly, + "FerramAerospaceResearch.FARAeroUtil", + "RayleighPitotTubeStagPressure", + BindingFlags.Public | BindingFlags.Static, + new Type[] { typeof(double) }); + + if (rayleighPitotTubeStagPressureMethodInfo == null) + { + Debug.LogWarning("[AtmosphereAutopilot]: FAR RayleighPitotTubeStagPressure() method reflection failed, reverting to EAS speed conversions"); + } + else + { + RayleighPitotTubeStagPressureDelegate = (RayleighPitotTubeStagPressureDelegateType) + Delegate.CreateDelegate( + typeof(RayleighPitotTubeStagPressureDelegateType), + rayleighPitotTubeStagPressureMethodInfo); + } + } + } + } +} diff --git a/AtmosphereAutopilot/Reflections/ReflectionUtils.cs b/AtmosphereAutopilot/Reflections/ReflectionUtils.cs new file mode 100644 index 0000000..4303274 --- /dev/null +++ b/AtmosphereAutopilot/Reflections/ReflectionUtils.cs @@ -0,0 +1,58 @@ +/* +Atmosphere Autopilot, plugin for Kerbal Space Program. +Copyright (C) 2015-2016, Baranin Alexander aka Boris-Barboris. + +Atmosphere Autopilot is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. +Atmosphere Autopilot is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. +You should have received a copy of the GNU General Public License +along with Atmosphere Autopilot. If not, see . +*/ + + +using System; +using System.Linq; +using System.Reflection; + +namespace AtmosphereAutopilot +{ + public static class ReflectionUtils + { + public static Assembly GetAssembly(string module) + { + foreach (var a in AppDomain.CurrentDomain.GetAssemblies()) + { + if (a.GetName().Name.Equals(module)) + { + return a; + } + } + return null; + } + public static MethodInfo GetMethodInfo(Assembly assembly, string className, string methodName, BindingFlags flags, Type[] args = null) + { + if (assembly == null) + return null; + + Type type = assembly.GetType(className, false); + + if (type == null) + return null; + + if (args == null) + args = Type.EmptyTypes; + + return type.GetMethod(methodName, flags, null, args, null); + } + + public static Type GetType(Assembly assembly, string typeName) + { + return assembly.GetTypes().First(t => t.Name.Equals(typeName)); + } + } +}