/// <summary> /// Load infos into this object and create a new BaseEvent /// </summary> /// <returns>true - loaded successfull</returns> public override bool Load(ConfigNode n, FlightComputer fc) { if(base.Load(n, fc)) { // deprecated since 1.6.2, we need this for upgrading from 1.6.x => 1.6.2 int PartId = 0; { if (n.HasValue("PartId")) PartId = int.Parse(n.GetValue("PartId")); } if (n.HasValue("flightID")) this.flightID = uint.Parse(n.GetValue("flightID")); this.Module = n.GetValue("Module"); this.GUIName = n.GetValue("GUIName"); this.Name = n.GetValue("Name"); RTLog.Notify("Try to load an EventCommand from persistent with {0},{1},{2},{3},{4}", PartId, this.flightID, this.Module, this.GUIName, this.Name); Part part = null; var partlist = FlightGlobals.ActiveVessel.parts; if (this.flightID == 0) { // only look with the partid if we've enough parts if (PartId < partlist.Count) part = partlist.ElementAt(PartId); } else { part = partlist.Where(p => p.flightID == this.flightID).FirstOrDefault(); } if (part == null) return false; PartModule partmodule = part.Modules[Module]; if (partmodule == null) return false; BaseEventList eventlist = new BaseEventList(part, partmodule); if (eventlist.Count <= 0) return false; this.BaseEvent = eventlist.Where(ba => (ba.GUIName == this.GUIName || ba.name == this.Name)).FirstOrDefault(); return true; } return false; }
public override bool Pop(FlightComputer f) { if (this.BaseEvent != null) { try { // invoke the baseevent this.BaseEvent.Invoke(); } catch (Exception invokeException) { RTLog.Notify("BaseEvent invokeException by '{0}' with message: {1}", RTLogLevel.LVL1, this.BaseEvent.guiName, invokeException.Message); } } return false; }
public override bool Pop(FlightComputer f) { if (BaseField != null) { try { var field = (BaseField as WrappedField); if (field == null) // we lost the Wrapped field instance, this is due to the fact that the command was loaded from a save { if (NewValue != null) { var newfield = new WrappedField(BaseField, WrappedField.KspFieldFromBaseField(BaseField)); if(newfield.NewValueFromString(NewValueString)) { newfield.Invoke(); } } } else { // invoke the field value change field.Invoke(); } if (UIPartActionController.Instance != null) UIPartActionController.Instance.UpdateFlight(); } catch (Exception invokeException) { RTLog.Notify("BaseField InvokeAction() by '{0}' with message: {1}", RTLogLevel.LVL1, this.BaseField.guiName, invokeException.Message); } } return false; }
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra) { var v = f.Vessel; var forward = Vector3.zero; var up = Vector3.zero; bool ignoreRoll = false; switch (frame) { case ReferenceFrame.Orbit: ignoreRoll = true; forward = v.GetObtVelocity(); up = (v.mainBody.position - v.CoM); break; case ReferenceFrame.Surface: ignoreRoll = true; forward = v.GetSrfVelocity(); up = (v.mainBody.position - v.CoM); break; case ReferenceFrame.North: up = (v.mainBody.position - v.CoM); forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up); break; case ReferenceFrame.Maneuver: ignoreRoll = true; if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0) { forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit); up = (v.mainBody.position - v.CoM); } else { forward = v.GetObtVelocity(); up = (v.mainBody.position - v.CoM); } break; case ReferenceFrame.TargetVelocity: // f.DelayedTarget may be any ITargetable, including a planet // Velocity matching only makes sense for vessels and part modules // Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody) if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody)) { forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity(); up = (v.mainBody.position - v.CoM); } else { up = (v.mainBody.position - v.CoM); forward = v.GetObtVelocity(); } break; case ReferenceFrame.TargetParallel: if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody)) { forward = f.DelayedTarget.GetTransform().position - v.CoM; up = (v.mainBody.position - v.CoM); } else { up = (v.mainBody.position - v.CoM); forward = v.GetObtVelocity(); } break; } Vector3.OrthoNormalize(ref forward, ref up); Quaternion rotationReference = Quaternion.LookRotation(forward, up); switch (attitude) { case FlightAttitude.Prograde: break; case FlightAttitude.Retrograde: rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up); break; case FlightAttitude.NormalPlus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up); break; case FlightAttitude.NormalMinus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down); break; case FlightAttitude.RadialPlus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right); break; case FlightAttitude.RadialMinus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left); break; case FlightAttitude.Surface: rotationReference = rotationReference * extra; break; } HoldOrientation(fs, f, rotationReference, ignoreRoll); }
/// <summary> /// Automatically guides the ship to face a desired orientation /// </summary> /// <param name="target">The desired orientation</param> /// <param name="c">The FlightCtrlState for the current vessel.</param> /// <param name="fc">The flight computer carrying out the slew</param> /// <param name="ignoreRoll">[optional] to ignore the roll</param> public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll) { // Add support for roll-less targets later -- Starstrider42 bool fixedRoll = !ignoreRoll; Vessel vessel = fc.Vessel; Vector3d momentOfInertia = GetTrueMoI(vessel); Transform vesselReference = vessel.GetTransform(); //--------------------------------------- // Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42 Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target); Vector3d torque = GetTorque(vessel, c.mainThrottle); Vector3d spinMargin = GetStoppingAngle(vessel, torque); // Allow for zero torque around some but not all axes Vector3d normFactor; normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0); normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0); normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0); normFactor = SwapYZ(normFactor); // Find out the real shorter way to turn were we want to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0f; var err = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, fixedRoll ? ((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F ); err += SwapYZ(spinMargin); err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(normFactor); // angular velocity: Vector3d omega = SwapYZ(vessel.angularVelocity); omega.Scale(normFactor); Vector3d pidAction = fc.pid.Compute(err, omega); // low pass filter, wf = 1/Tf: Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1)); fc.lastAct = act; // end MechJeb import //--------------------------------------- float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f); float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision)); act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit); act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit); act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit); c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit); c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit); c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit); }
public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quaternion target, bool ignoreRoll = false) { f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false); SteeringHelper.SteerShipToward(target, fs, f, ignoreRoll); }
/// <summary> /// Save the BaseEvent to the persistent /// </summary> public override void Save(ConfigNode n, FlightComputer fc) { PartModule pm = (BaseField.host as PartModule); if(pm == null) { RTLog.Notify("On PartActionCommand.Save(): Can't save because BaseField.host is not a PartModule instance. Type is: {0}", BaseField.host.GetType()); return; } GUIName = BaseField.guiName; flightID = pm.part.flightID; Module = pm.ClassName.ToString(); Name = BaseField.name; n.AddValue("NewValue", this.NewValue); base.Save(n, fc); }
private static void SetPIDParameters(FlightComputer fc, Vector3d TfV, double kdFactor, double kpFactor, double kiFactor) { var pid = fc.pid; Vector3d invTf = TfV.InvertNoNaN(); pid.Kd = kdFactor * invTf; pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * pid.Kd; pid.Kp.Scale(invTf); pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * pid.Kp; pid.Ki.Scale(invTf); pid.intAccum = pid.intAccum.Clamp(-5, 5); }
/// <summary> /// Automatically guides the ship to face a desired orientation /// </summary> /// <param name="target">The desired orientation</param> /// <param name="c">The FlightCtrlState for the current vessel.</param> /// <param name="fc">The flight computer carrying out the slew</param> /// <param name="ignoreRoll">[optional] to ignore the roll</param> public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll) { // Add support for roll-less targets later -- Starstrider42 bool fixedRoll = !ignoreRoll; Vessel vessel = fc.Vessel; Vector3d momentOfInertia = vessel.MOI; Transform vesselReference = vessel.GetTransform(); Vector3d torque = GetVesselTorque(vessel); // ----------------------------------------------- // Copied from MechJeb master on 18.04.2016 with some modifications to adapt to RemoteTech Vector3d _axisControl = new Vector3d(); _axisControl.x = true ? 1 : 0; _axisControl.y = true ? 1 : 0; _axisControl.z = fixedRoll ? 1 : 0; Vector3d inertia = Vector3d.Scale( new Vector3d(vessel.angularMomentum.x, vessel.angularMomentum.y, vessel.angularMomentum.z).Sign(), Vector3d.Scale( Vector3d.Scale(vessel.angularMomentum, vessel.angularMomentum), Vector3d.Scale(torque, momentOfInertia).Invert() ) ); Vector3d TfV = new Vector3d(0.3, 0.3, 0.3); double kpFactor = 3; double kiFactor = 6; double kdFactor = 0.5; double kWlimit = 0.15; double deadband = 0.0001; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target); Vector3d deltaEuler = delta.DeltaEuler(); // ( MoI / available torque ) factor: Vector3d NormFactor = Vector3d.Scale(momentOfInertia, torque.Invert()).Reorder(132); // Find out the real shorter way to turn were we want to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselReference.rotation.Inverse() * target * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0; // And the lowest roll // Thanks to Crzyrndm Vector3 normVec = Vector3.Cross(target * Vector3.forward, vesselReference.up); Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * target; float rollError = Vector3.Angle(vesselReference.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselReference.forward)); var error = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, rollError * Mathf.Deg2Rad ); error.Scale(_axisControl); Vector3d err = error + inertia.Reorder(132) / 2d; err = new Vector3d( Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); //if (Tf_autoTune) // tuneTf(torque); Vector3d invTf = TfV.Invert(); fc.pid.Kd = kdFactor * invTf; fc.pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * fc.pid.Kd; fc.pid.Kp.Scale(invTf); fc.pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * fc.pid.Kp; fc.pid.Ki.Scale(invTf); fc.pid.intAccum = fc.pid.intAccum.Clamp(-5, 5); // angular velocity limit: var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit), Math.Sqrt(NormFactor.y * Math.PI * kWlimit), Math.Sqrt(NormFactor.z * Math.PI * kWlimit)); Vector3d pidAction = fc.pid.Compute(err, omega, Wlimit); // deadband pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0; pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0; pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0; // low pass filter, wf = 1/Tf: Vector3d act = fc.lastAct; act.x += (pidAction.x - fc.lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0)); act.y += (pidAction.y - fc.lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0)); act.z += (pidAction.z - fc.lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0)); fc.lastAct = act; // end MechJeb import //--------------------------------------- float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f); float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision)); act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit); act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit); act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit); c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit); c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit); c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit); }
/// <summary> /// Single entry point of all Flight Computer orientation holding, including maneuver node. /// </summary> public static void HoldOrientation(FlightCtrlState fs, FlightComputer f, Quaternion target, bool ignoreRoll = false) { f.Vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false); SteeringHelper.SteerShipToward(target, fs, f, ignoreRoll); }
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra) { var v = f.Vessel; var forward = Vector3.zero; var up = Vector3.zero; bool ignoreRoll = false; switch (frame) { case ReferenceFrame.Orbit: ignoreRoll = true; forward = v.GetObtVelocity(); up = (v.mainBody.position - v.CoM); break; case ReferenceFrame.Surface: ignoreRoll = true; forward = v.GetSrfVelocity(); up = (v.mainBody.position - v.CoM); break; case ReferenceFrame.North: up = (v.mainBody.position - v.CoM); forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up); break; case ReferenceFrame.Maneuver: ignoreRoll = true; if (f.Vessel.patchedConicSolver == null) //scenario: two vessels within physical range with FC attitude hold cmds. Unloaded one doesn't have solver instance { f.Vessel.AttachPatchedConicsSolver(); f.Vessel.patchedConicSolver.Update(); } if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0) { forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit); up = (v.mainBody.position - v.CoM); } else { forward = v.GetObtVelocity(); up = (v.mainBody.position - v.CoM); } break; case ReferenceFrame.TargetVelocity: // f.DelayedTarget may be any ITargetable, including a planet // Velocity matching only makes sense for vessels and part modules // Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody) if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody)) { forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity(); up = (v.mainBody.position - v.CoM); } else { up = (v.mainBody.position - v.CoM); forward = v.GetObtVelocity(); } break; case ReferenceFrame.TargetParallel: if (f.DelayedTarget != null) // either target vessel or celestial body { forward = f.DelayedTarget.GetTransform().position - v.CoM; up = (v.mainBody.position - v.CoM); } else // no target to aim; default to orbital prograde { up = (v.mainBody.position - v.CoM); forward = v.GetObtVelocity(); } break; } Vector3.OrthoNormalize(ref forward, ref up); Quaternion rotationReference = Quaternion.LookRotation(forward, up); switch (attitude) { case FlightAttitude.Prograde: break; case FlightAttitude.Retrograde: rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up); break; case FlightAttitude.NormalPlus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up); break; case FlightAttitude.NormalMinus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down); break; case FlightAttitude.RadialPlus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right); break; case FlightAttitude.RadialMinus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left); break; case FlightAttitude.Surface: rotationReference = rotationReference * extra; break; } HoldOrientation(fs, f, rotationReference, ignoreRoll); }
public static void HoldAttitude(FlightCtrlState fs, FlightComputer f, ReferenceFrame frame, FlightAttitude attitude, Quaternion extra) { var v = f.Vessel; var forward = Vector3.zero; var up = Vector3.zero; switch (frame) { case ReferenceFrame.Orbit: forward = v.GetObtVelocity(); up = (v.mainBody.position - v.CoM); break; case ReferenceFrame.Surface: forward = v.GetSrfVelocity(); up = (v.mainBody.position - v.CoM); break; case ReferenceFrame.North: up = (v.mainBody.position - v.CoM); forward = Vector3.Exclude(up, v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM ); break; case ReferenceFrame.Maneuver: if (f.Vessel.patchedConicSolver.maneuverNodes.Count != 0) { forward = f.Vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit); up = (v.mainBody.position - v.CoM); } else { forward = v.GetObtVelocity(); up = (v.mainBody.position - v.CoM); } break; case ReferenceFrame.TargetVelocity: // f.DelayedTarget may be any ITargetable, including a planet // Velocity matching only makes sense for vessels and part modules // Can test for Vessel but not PartModule, so instead test that it's not the third case (CelestialBody) if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody)) { forward = v.GetObtVelocity() - f.DelayedTarget.GetObtVelocity(); up = (v.mainBody.position - v.CoM); } else { up = (v.mainBody.position - v.CoM); forward = v.GetObtVelocity(); } break; case ReferenceFrame.TargetParallel: if (f.DelayedTarget != null && !(f.DelayedTarget is CelestialBody)) { forward = f.DelayedTarget.GetTransform().position - v.CoM; up = (v.mainBody.position - v.CoM); } else { up = (v.mainBody.position - v.CoM); forward = v.GetObtVelocity(); } break; } Vector3.OrthoNormalize(ref forward, ref up); Quaternion rotationReference = Quaternion.LookRotation(forward, up); switch (attitude) { case FlightAttitude.Prograde: break; case FlightAttitude.Retrograde: rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up); break; case FlightAttitude.NormalPlus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.up); break; case FlightAttitude.NormalMinus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.down); break; case FlightAttitude.RadialPlus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.right); break; case FlightAttitude.RadialMinus: rotationReference = rotationReference * Quaternion.AngleAxis(90, Vector3.left); break; case FlightAttitude.Surface: rotationReference = rotationReference * extra; break; } HoldOrientation(fs, f, rotationReference); }
/// <summary> /// Automatically guides the ship to face a desired orientation /// </summary> /// <param name="target">The desired orientation</param> /// <param name="c">The FlightCtrlState for the current vessel.</param> /// <param name="fc">The flight computer carrying out the slew</param> public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc) { // Add support for roll-less targets later -- Starstrider42 bool fixedRoll = true; Vessel vessel = fc.Vessel; Vector3d momentOfInertia = GetTrueMoI(vessel); Transform vesselReference = vessel.GetTransform(); //--------------------------------------- // Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42 Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target); Vector3d torque = GetTorque(vessel, c.mainThrottle); Vector3d spinMargin = GetStoppingAngle(vessel, torque); // Allow for zero torque around some but not all axes Vector3d normFactor; normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0); normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0); normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0); normFactor = SwapYZ(normFactor); // Find out the real shorter way to turn were we want to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0f; var err = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, fixedRoll ? ((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F ); err += SwapYZ(spinMargin); err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(normFactor); // angular velocity: Vector3d omega = SwapYZ(vessel.angularVelocity); omega.Scale(normFactor); Vector3d pidAction = fc.pid.Compute(err, omega); // low pass filter, wf = 1/Tf: Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1)); fc.lastAct = act; // end MechJeb import //--------------------------------------- float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f); float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision)); act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit); act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit); act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit); c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit); c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit); c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit); }
/// <summary> /// Save the BaseEvent to the persistent /// </summary> public override void Save(ConfigNode n, FlightComputer fc) { this.GUIName = this.BaseEvent.GUIName; this.flightID = this.BaseEvent.listParent.module.part.flightID; this.Module = this.BaseEvent.listParent.module.ClassName.ToString(); this.Name = this.BaseEvent.name; base.Save(n, fc); }
/// <summary> /// Load infos into this object and create a new BaseEvent /// </summary> /// <returns>true - loaded successfull</returns> public override bool Load(ConfigNode n, FlightComputer fc) { if (base.Load(n, fc)) { // deprecated since 1.6.2, we need this for upgrading from 1.6.x => 1.6.2 int PartId = 0; { if (n.HasValue("PartId")) { PartId = int.Parse(n.GetValue("PartId")); } } if (n.HasValue("flightID")) { this.flightID = uint.Parse(n.GetValue("flightID")); } this.Module = n.GetValue("Module"); this.GUIName = n.GetValue("GUIName"); this.Name = n.GetValue("Name"); RTLog.Notify("Try to load an EventCommand from persistent with {0},{1},{2},{3},{4}", PartId, this.flightID, this.Module, this.GUIName, this.Name); Part part = null; var partlist = FlightGlobals.ActiveVessel.parts; if (this.flightID == 0) { // only look with the partid if we've enough parts if (PartId < partlist.Count) { part = partlist.ElementAt(PartId); } } else { part = partlist.Where(p => p.flightID == this.flightID).FirstOrDefault(); } if (part == null) { return(false); } PartModule partmodule = part.Modules[Module]; if (partmodule == null) { return(false); } BaseEventList eventlist = new BaseEventList(part, partmodule); if (eventlist.Count <= 0) { return(false); } this.BaseEvent = eventlist.Where(ba => (ba.GUIName == this.GUIName || ba.name == this.Name)).FirstOrDefault(); return(true); } return(false); }