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)); + } + } +}